├── MIT-License.txt ├── README.md ├── vgg_examples ├── F_from_Ps_ex.m ├── H_from_image_corr_ex.m ├── Haffine_from_x_MLE_ex.m ├── bt.00.02.H ├── bt.000.png ├── bt.002.png ├── bt.004.png ├── bt.006.png ├── chapel.00.01.F ├── chapel00.png ├── chapel01.png ├── keble.000.png ├── keble.003.png ├── ransacfithomography_vgg.m ├── ransacfithomography_vgg_bak.m ├── rms.ps ├── testhomog_vgg.m ├── testhomog_vgg_bak.m ├── vgg_example_scene.m ├── view_fund_ex.m └── view_homog_ex.m ├── vgg_general └── vgg_argparse.m ├── vgg_multiview ├── .DS_Store ├── Contents.m ├── private │ ├── .DS_Store │ ├── CVS │ │ ├── Entries │ │ ├── Repository │ │ └── Root │ ├── vgg_H_sampson_distance_sqr.m │ ├── vgg_condition_2d.m │ ├── vgg_decondition_2d.m │ └── vgg_singF_from_FF.m ├── vgg_F_from_7pts_2img.m ├── vgg_F_from_P.m ├── vgg_H_algebraic_distance.m ├── vgg_H_from_2P_plane.m ├── vgg_H_from_P_plane.m ├── vgg_H_from_x_lin.m ├── vgg_H_from_x_nonlin.m ├── vgg_H_sampson_distance_sqr.m ├── vgg_Haffine_from_x_MLE.m ├── vgg_KR_from_P.m ├── vgg_PX_from_6pts_3img.m ├── vgg_P_from_F.m ├── vgg_T_from_P.m ├── vgg_X_from_xP_lin.m ├── vgg_X_from_xP_nonlin.m ├── vgg_conditioner_from_image.m ├── vgg_conditioner_from_pts.m ├── vgg_fit_hplane_to_x.m ├── vgg_get_homg.m ├── vgg_get_nonhomg.m ├── vgg_line3d_Ppv.m ├── vgg_line3d_XY_from_pm.m ├── vgg_line3d_XY_from_pv.m ├── vgg_line3d_from_lP_lin.m ├── vgg_line3d_from_lP_nonlin.m ├── vgg_line3d_pm_from_pv.m ├── vgg_line3d_pv_from_2planes.m ├── vgg_line3d_pv_from_XY.m ├── vgg_line3d_pv_from_pm.m ├── vgg_plane_from_2P_H.m ├── vgg_poly3d_orthorectify.m ├── vgg_projective_basis_2d.m ├── vgg_rms_error.m ├── vgg_scatter_plot.m ├── vgg_scatter_plot_homg.m ├── vgg_selfcalib_metric_vansq.m ├── vgg_selfcalib_qaffine.m └── vgg_signsPX_from_x.m ├── vgg_numerics ├── Contents.m ├── vgg_commut_matrix.m ├── vgg_contreps.m ├── vgg_detn.cxx ├── vgg_detn.dll ├── vgg_diagonalize_conic.m ├── vgg_duplic_matrix.m ├── vgg_gauss_mask.m ├── vgg_intersect_quadrics.m ├── vgg_lmultiply_matrix.m ├── vgg_matrix_test.m ├── vgg_mrdivs.m ├── vgg_quat_from_rotation_matrix.m ├── vgg_quat_matrix.m ├── vgg_quat_mul.m ├── vgg_quat_rotation_matrix.m ├── vgg_quaternion_demo.m ├── vgg_rmultiply_matrix.m ├── vgg_rotmat_from_exp.m ├── vgg_rq.m ├── vgg_solvelin_blksym.m ├── vgg_vec.m ├── vgg_vec_swap.m ├── vgg_vech.m ├── vgg_vech_swap.m └── vgg_wedge.m └── vgg_ui ├── Contents.m ├── vgg_gui_F.m └── vgg_gui_H.m /MIT-License.txt: -------------------------------------------------------------------------------- 1 | License for 2 | "MATLAB Functions for Multiple View Geometry" 3 | 4 | 5 | Copyright (c) 1995-2005 Visual Geometry Group 6 | Department of Engineering Science 7 | University of Oxford 8 | http://www.robots.ox.ac.uk/~vgg/ 9 | 10 | 11 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 14 | 15 | The software is provided "as is", without warranty of any kind, express or implied, including but not limited to the warranties of merchantability, fitness for a particular purpose and noninfringement. In no event shall the authors or copyright holders be liable for any claim, damages or other liability, whether in an action of contract, tort or otherwise, arising from, out of or in connection with the software or the use or other dealings in the software. 16 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MATLAB Functions for Multiple View Geometry 2 | 3 | Obtained from [http://www.robots.ox.ac.uk/~vgg/hzbook/code/](http://www.robots.ox.ac.uk/~vgg/hzbook/code/). 4 | 5 | Please report any bugs to [Andrew Zisserman](removethisifyouarehuman-az@robots.ox.ac.uk). 6 | 7 | 8 | ### Acknowledgements: 9 | These functions are written by: David Capel, Andrew Fitzgibbon, Peter Kovesi, Tomas Werner, Yoni Wexler, and Andrew Zisserman 10 | 11 | 12 | 13 | ## VGG MultiView Compute Library 14 | 15 | ### Conversions: 16 | * `vgg_KR_from_P.m` extract K, R from P such that P = K*R*[eye(3) -t] 17 | * `vgg_F_from_P.m` fundamental matrix from 2 cameras 18 | * `vgg_P_from_F.m` 2 camera matrices from fundamental matrix 19 | * `vgg_T_from_P.m` trifocal tensor from 3 cameras 20 | * `vgg_H_from_2P_plane.m` inter-image homography from 2 cameras and 3D plane 21 | * `vgg_H_from_P_plane.m` projection matrix from image onto 3D plane 22 | * `vgg_plane_from_2P_H.m` 3D plane from 2 cameras and inter-image homography 23 | 24 | ### Multiview tensors from image correspondences: 25 | * `vgg_H_from_x_lin.m` homography from points in 2 images, linear method 26 | * `vgg_H_from_x_nonlin.m` MLE of the above, by nonlinear method 27 | * `vgg_Haffine_from_x_MLE.m` MLE of affine transformation from points in 2 images, linear 28 | * `vgg_F_from_7pts_2img.m` fundamental matrix from 7 points in 2 images 29 | * `vgg_PX_from_6pts_3img.m` cameras and world points from 6 points in 3 images 30 | 31 | ### Preconditioning for estimation: 32 | * `vgg_conditioner_from_image.m` conditioning shift+scaling from image dimensions 33 | * `vgg_conditioner_from_pts.m` conditioning shift+scaling from image points 34 | 35 | ### Self-calibration and similar: 36 | * `vgg_signsPX_from_x.m` swaps signs of P and X so that projection scales are positive 37 | * `vgg_selfcalib_qaffine.m` quasi-affine from projective reconstruction 38 | * `vgg_selfcalib_metric_vansq.m` metric from projective and 3 orthogonal principal directions and square pixels 39 | 40 | ### Estimation: 41 | * `vgg_X_from_xP_lin.m` 3D point from image projections and cameras, linear 42 | * `vgg_X_from_xP_nonlin.m` MLE of that, non-linear method 43 | * `vgg_line3d_from_lP_lin.m` 3D line segment from image line segments and cameras, linear 44 | * `vgg_line3d_from_lP_nonlin.m` MLE of that, non-linear method 45 | 46 | 47 | 48 | ## VGG User Interface Library 49 | 50 | ### GUI’s: 51 | * `vgg_gui_F.m` Visualizes epipolar geometry between two views 52 | * `vgg_gui_H.m` Visualizes a homography between two views 53 | 54 | 55 | 56 | ## Examples 57 | 58 | These examples use images and matrices included in the directory `vgg_examples`. Change to that directory before running the example functions. 59 | 60 | * `view_homog_ex.m` Example of using `vgg_gui_H` 61 | * `view_fund_ex.m` Example of using `vgg_gui_F` 62 | * `Haffine_from_x_MLE_ex.m` Example of using `vgg_Haffine_from_x_MLE` 63 | * `F_from_Ps_ex.m` Example on computing F from two camera matrices using `vgg_F_from_P` 64 | * `H_from_image_corr_ex.m` Example on computing H from points using `vgg_H_from_x_lin` 65 | * `testhomog_vgg.m` Example of computing H from two images from a rotating camera. This example also requires `ransacfithomography_vgg.m` and Peter Kovesi's functions (such as `matchbycorrelation.m` and `ransac.m`). See link below. 66 | 67 | 68 | 69 | ## Links to other highly recommended Computer Vision software 70 | 71 | * [Peter Kovesi's Matlab Functions for Computer Vision and Image Analysis](http://www.csse.uwa.edu.au/~pk/Research/MatlabFns/index.html) 72 | * [Jean-Yves Bouguet's Matlab Calibration Software](http://www.vision.caltech.edu/bouguetj/calib_doc/) 73 | 74 | 75 | 76 | ## Note on release versions 77 | 78 | November 2012 updates to 79 | 80 | * `ransacfithomography_vgg.m` 81 | * `testhomog_vgg.m` 82 | * `vgg_H_from_x_nonlin.m` 83 | 84 | To maintain compatibility with Peter Kovesi's functions and for Matlab R2012a compatibility. 85 | 86 | Thanks to: Relja Arandjelovic, Peter Corke and Alexander Khanin. -------------------------------------------------------------------------------- /vgg_examples/F_from_Ps_ex.m: -------------------------------------------------------------------------------- 1 | % Example on computing F from two camera matrices using vgg_F_from_P 2 | % and displaying result 3 | 4 | % Note P's are read from stored example in vgg_example_scene 5 | 6 | % Read in P's and images 7 | [view] = vgg_example_scene(2); 8 | 9 | % Compute F from P's 10 | F = vgg_F_from_P(view(1).P, view(2).P); 11 | 12 | % Display 13 | vgg_gui_F(view(1).I, view(2).I, F'); 14 | disp('Computed epipolar geometry. Move the mouse to verify') 15 | -------------------------------------------------------------------------------- /vgg_examples/H_from_image_corr_ex.m: -------------------------------------------------------------------------------- 1 | % Example on computing H from a set correspondences for planar points (on the 2 | % floor) using vgg_H_from_x_lin and displaying result 3 | 4 | % Note points are read from stored example in vgg_example_scene 5 | 6 | % Read in all detected interest points and images 7 | [view, Xi, X, Li, L] = vgg_example_scene(2); 8 | 9 | n=1; k=2; 10 | 11 | % select points for which there are correspondences 12 | i=all(Xi([n k],:)>0); i=find(i); 13 | 14 | % compute H from points on the floor 15 | H=vgg_H_from_x_lin(view(n).x(:,Xi(n,i([406 399 405 133 115 109]))),... 16 | view(k).x(:,Xi(k,i([406 399 405 133 115 109])))); 17 | 18 | % Display 19 | vgg_gui_H(view(n).I, view(k).I, H) 20 | disp('Computed homography of the floor. Move the mouse to verify') 21 | -------------------------------------------------------------------------------- /vgg_examples/Haffine_from_x_MLE_ex.m: -------------------------------------------------------------------------------- 1 | % Example of using vgg_Haffine_from_x_MLE 2 | 3 | % data is square, rotated square 4 | 5 | exem1 = [ 0 1 1 0 ; 0 0 1 1 ]; 6 | exem2 = [ 0.5 1 0.5 0; 0 0.5 1 0.5 ]; 7 | 8 | H = vgg_Haffine_from_x_MLE(vgg_get_homg(exem1) ,vgg_get_homg(exem2)); 9 | disp('Affine H relating two noise free point sets:'); 10 | disp(H); 11 | 12 | % add noise 13 | 14 | dat = 0.05 * randn(2,4); 15 | exem1 = exem1 + dat; 16 | 17 | dat = 0.05 * randn(2,4); 18 | exem2 = exem2 + dat; 19 | 20 | H = vgg_Haffine_from_x_MLE(vgg_get_homg(exem1) ,vgg_get_homg(exem2)); 21 | disp(' ') 22 | disp('MLE estimate of affine H relating the point sets with added noise:'); 23 | disp(H); 24 | -------------------------------------------------------------------------------- /vgg_examples/bt.00.02.H: -------------------------------------------------------------------------------- 1 | 9.0750383350423003e-001 -1.1649657888193843e-001 3.0847191818192250e+001 2 | 3.0807286021607137e-003 8.2881598946924762e-001 1.6044853701519866e+001 3 | -1.7401501350741370e-005 -4.4172103260319348e-004 1.0000000000000000e+000 4 | -------------------------------------------------------------------------------- /vgg_examples/bt.000.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jmmanley/VGG-Multiple-View-Geometry/f114712de03082bb97229eaf2a65981908b64127/vgg_examples/bt.000.png -------------------------------------------------------------------------------- /vgg_examples/bt.002.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jmmanley/VGG-Multiple-View-Geometry/f114712de03082bb97229eaf2a65981908b64127/vgg_examples/bt.002.png -------------------------------------------------------------------------------- /vgg_examples/bt.004.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jmmanley/VGG-Multiple-View-Geometry/f114712de03082bb97229eaf2a65981908b64127/vgg_examples/bt.004.png -------------------------------------------------------------------------------- /vgg_examples/bt.006.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jmmanley/VGG-Multiple-View-Geometry/f114712de03082bb97229eaf2a65981908b64127/vgg_examples/bt.006.png -------------------------------------------------------------------------------- /vgg_examples/chapel.00.01.F: -------------------------------------------------------------------------------- 1 | 1.716731680428799018e-09 -1.2598565711090784764e-07 3.1618609316601319157e-05 2 | 1.7611895205274598726e-07 -8.5531573537983242911e-09 0.00039292998964143144433 3 | -4.131229744327214083e-05 -0.00040521774832684681177 -0.0022893781155071679656 4 | -------------------------------------------------------------------------------- /vgg_examples/chapel00.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jmmanley/VGG-Multiple-View-Geometry/f114712de03082bb97229eaf2a65981908b64127/vgg_examples/chapel00.png -------------------------------------------------------------------------------- /vgg_examples/chapel01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jmmanley/VGG-Multiple-View-Geometry/f114712de03082bb97229eaf2a65981908b64127/vgg_examples/chapel01.png -------------------------------------------------------------------------------- /vgg_examples/keble.000.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jmmanley/VGG-Multiple-View-Geometry/f114712de03082bb97229eaf2a65981908b64127/vgg_examples/keble.000.png -------------------------------------------------------------------------------- /vgg_examples/keble.003.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jmmanley/VGG-Multiple-View-Geometry/f114712de03082bb97229eaf2a65981908b64127/vgg_examples/keble.003.png -------------------------------------------------------------------------------- /vgg_examples/ransacfithomography_vgg.m: -------------------------------------------------------------------------------- 1 | % RANSACFITHOMOGRAPHY - fits 2D homography using RANSAC 2 | % 3 | % Usage: [H, inliers] = ransacfithomography_vgg(x1, x2, t) 4 | % 5 | % Arguments: 6 | % x1 - 2xN or 3xN set of homogeneous points. If the data is 7 | % 2xN it is assumed the homogeneous scale factor is 1. 8 | % x2 - 2xN or 3xN set of homogeneous points such that x1<->x2. 9 | % t - The distance threshold between data point and the model 10 | % used to decide whether a point is an inlier or not. 11 | % Note that point coordinates are normalised to that their 12 | % mean distance from the origin is sqrt(2). The value of 13 | % t should be set relative to this, say in the range 14 | % 0.001 - 0.01 15 | % 16 | % Note that it is assumed that the matching of x1 and x2 are putative and it 17 | % is expected that a percentage of matches will be wrong. 18 | % 19 | % Returns: 20 | % H - The 3x3 homography such that x2 = H*x1. 21 | % inliers - An array of indices of the elements of x1, x2 that were 22 | % the inliers for the best model. 23 | % 24 | % See Also: ransac, homography2d, homography1d 25 | 26 | % Peter Kovesi 27 | % School of Computer Science & Software Engineering 28 | % The University of Western Australia 29 | % pk at csse uwa edu au 30 | % http://www.csse.uwa.edu.au/~pk 31 | % 32 | % February 2004 - original version 33 | % July 2004 - error in denormalising corrected (thanks to Andrew Stein) 34 | 35 | % Adapted to use vgg functions by Peter Kovesi and Andrew Zisserman 36 | % 2012 Updated by Alexander Khanin 37 | 38 | function [H, inliers] = ransacfithomography_vgg(x1, x2, t) 39 | 40 | if ~all(size(x1)==size(x2)) 41 | error('Data sets x1 and x2 must have the same dimension'); 42 | end 43 | 44 | [rows,npts] = size(x1); 45 | if rows ~= 3 46 | error('x1 and x2 must have 3 rows'); 47 | end 48 | 49 | if npts < 4 50 | error('Must have at least 4 points to fit homography'); 51 | end 52 | 53 | % Normalise each set of points so that the origin is at centroid and 54 | % mean distance from origin is sqrt(2). normalise2dpts also ensures the 55 | % scale parameter is 1. Note that 'homography2d' will also call 56 | % 'normalise2dpts' but the code in 'ransac' that calls the distance 57 | % function will not - so it is best that we normalise beforehand. 58 | [x1, T1] = normalise2dpts(x1); 59 | [x2, T2] = normalise2dpts(x2); 60 | 61 | s = 4; % Minimum No of points needed to fit a homography. 62 | 63 | fittingfn = @wrap_vgg_homography2d; 64 | distfn = @homogdist2d; 65 | degenfn = @isdegenerate; 66 | 67 | % x1 and x2 are 'stacked' to create a 6xN array for ransac 68 | [H, inliers] = ransac([x1; x2], fittingfn, distfn, degenfn, s, t); 69 | 70 | % Now do a final least squares fit on the data points considered to 71 | % be inliers. 72 | Hlin = vgg_H_from_x_lin(x1(:,inliers), x2(:,inliers)); 73 | H = vgg_H_from_x_nonlin(Hlin,x1(:,inliers), x2(:,inliers)); 74 | 75 | 76 | % Denormalise 77 | H = T2\H*T1; 78 | 79 | %---------------------------------------------------------------------- 80 | % Function to evaluate the symmetric transfer error of a homography with 81 | % respect to a set of matched points as needed by RANSAC. 82 | 83 | function [inliers, H] = homogdist2d(H, x, t) 84 | 85 | x1 = x(1:3,:); % Extract x1 and x2 from x 86 | x2 = x(4:6,:); 87 | 88 | % Calculate, in both directions, the transfered points 89 | Hx1 = H*x1; 90 | invHx2 = H\x2; 91 | 92 | % Normalise so that the homogeneous scale parameter for all coordinates 93 | % is 1. 94 | 95 | x1 = hnormalise(x1); 96 | x2 = hnormalise(x2); 97 | Hx1 = hnormalise(Hx1); 98 | invHx2 = hnormalise(invHx2); 99 | 100 | d2 = sum((x1-invHx2).^2) + sum((x2-Hx1).^2); 101 | inliers = find(abs(d2) < t); 102 | 103 | %---------------------------------------------------------------------- 104 | % Function to determine if a set of 4 pairs of matched points give rise 105 | % to a degeneracy in the calculation of a homography as needed by RANSAC. 106 | % This involves testing whether any 3 of the 4 points in each set is 107 | % colinear. 108 | 109 | function r = isdegenerate(x) 110 | 111 | x1 = x(1:3,:); % Extract x1 and x2 from x 112 | x2 = x(4:6,:); 113 | 114 | r = ... 115 | iscolinear(x1(:,1),x1(:,2),x1(:,3)) | ... 116 | iscolinear(x1(:,1),x1(:,2),x1(:,4)) | ... 117 | iscolinear(x1(:,1),x1(:,3),x1(:,4)) | ... 118 | iscolinear(x1(:,2),x1(:,3),x1(:,4)) | ... 119 | iscolinear(x2(:,1),x2(:,2),x2(:,3)) | ... 120 | iscolinear(x2(:,1),x2(:,2),x2(:,4)) | ... 121 | iscolinear(x2(:,1),x2(:,3),x2(:,4)) | ... 122 | iscolinear(x2(:,2),x2(:,3),x2(:,4)); 123 | 124 | function H = wrap_vgg_homography2d(x) 125 | 126 | H = vgg_H_from_x_lin(x(1:3,:),x(4:6,:)); 127 | -------------------------------------------------------------------------------- /vgg_examples/ransacfithomography_vgg_bak.m: -------------------------------------------------------------------------------- 1 | % RANSACFITHOMOGRAPHY - fits 2D homography using RANSAC 2 | % 3 | % Usage: [H, inliers] = ransacfithomography_vgg(x1, x2, t) 4 | % 5 | % Arguments: 6 | % x1 - 2xN or 3xN set of homogeneous points. If the data is 7 | % 2xN it is assumed the homogeneous scale factor is 1. 8 | % x2 - 2xN or 3xN set of homogeneous points such that x1<->x2. 9 | % t - The distance threshold between data point and the model 10 | % used to decide whether a point is an inlier or not. 11 | % Note that point coordinates are normalised to that their 12 | % mean distance from the origin is sqrt(2). The value of 13 | % t should be set relative to this, say in the range 14 | % 0.001 - 0.01 15 | % 16 | % Note that it is assumed that the matching of x1 and x2 are putative and it 17 | % is expected that a percentage of matches will be wrong. 18 | % 19 | % Returns: 20 | % H - The 3x3 homography such that x2 = H*x1. 21 | % inliers - An array of indices of the elements of x1, x2 that were 22 | % the inliers for the best model. 23 | % 24 | % See Also: ransac, homography2d, homography1d 25 | 26 | % Peter Kovesi 27 | % School of Computer Science & Software Engineering 28 | % The University of Western Australia 29 | % pk at csse uwa edu au 30 | % http://www.csse.uwa.edu.au/~pk 31 | % 32 | % February 2004 - original version 33 | % July 2004 - error in denormalising corrected (thanks to Andrew Stein) 34 | 35 | % Adapted to use vgg functions by Peter Kovesi and Andrew Zisserman 36 | 37 | function [H, inliers] = ransacfithomography_vgg(x1, x2, t) 38 | 39 | if ~all(size(x1)==size(x2)) 40 | error('Data sets x1 and x2 must have the same dimension'); 41 | end 42 | 43 | [rows,npts] = size(x1); 44 | if rows~=2 & rows~=3 45 | error('x1 and x2 must have 2 or 3 rows'); 46 | end 47 | 48 | if npts < 4 49 | error('Must have at least 4 points to fit homography'); 50 | end 51 | 52 | if rows == 2 % Pad data with homogeneous scale factor of 1 53 | x1 = [x1; ones(1,npts)]; 54 | x2 = [x2; ones(1,npts)]; 55 | end 56 | 57 | % Normalise each set of points so that the origin is at centroid and 58 | % mean distance from origin is sqrt(2). normalise2dpts also ensures the 59 | % scale parameter is 1. Note that 'homography2d' will also call 60 | % 'normalise2dpts' but the code in 'ransac' that calls the distance 61 | % function will not - so it is best that we normalise beforehand. 62 | [x1, T1] = normalise2dpts(x1); 63 | [x2, T2] = normalise2dpts(x2); 64 | 65 | s = 4; % Minimum No of points needed to fit a homography. 66 | 67 | fittingfn = @wrap_vgg_homography2d; 68 | distfn = @homogdist2d; 69 | degenfn = @isdegenerate; 70 | % x1 and x2 are 'stacked' to create a 6xN array for ransac 71 | [H, inliers] = ransac([x1; x2], fittingfn, distfn, degenfn, s, t); 72 | 73 | % Now do a final least squares fit on the data points considered to 74 | % be inliers. 75 | Hlin = vgg_H_from_x_lin(x1(:,inliers), x2(:,inliers)); 76 | H = vgg_H_from_x_nonlin(Hlin,x1(:,inliers), x2(:,inliers)); 77 | 78 | % Denormalise 79 | H = T2\H*T1; 80 | 81 | %---------------------------------------------------------------------- 82 | % Function to evaluate the symmetric transfer error of a homography with 83 | % respect to a set of matched points as needed by RANSAC. 84 | 85 | function d2 = homogdist2d(H, x); 86 | 87 | x1 = x(1:3,:); % Extract x1 and x2 from x 88 | x2 = x(4:6,:); 89 | 90 | % Calculate, in both directions, the transfered points 91 | Hx1 = H*x1; 92 | invHx2 = H\x2; 93 | 94 | % Normalise so that the homogeneous scale parameter for all coordinates 95 | % is 1. 96 | 97 | x1 = hnormalise(x1); 98 | x2 = hnormalise(x2); 99 | Hx1 = hnormalise(Hx1); 100 | invHx2 = hnormalise(invHx2); 101 | 102 | d2 = sum((x1-invHx2).^2) + sum((x2-Hx1).^2); 103 | 104 | 105 | %---------------------------------------------------------------------- 106 | % Function to determine if a set of 4 pairs of matched points give rise 107 | % to a degeneracy in the calculation of a homography as needed by RANSAC. 108 | % This involves testing whether any 3 of the 4 points in each set is 109 | % colinear. 110 | 111 | function r = isdegenerate(x) 112 | 113 | x1 = x(1:3,:); % Extract x1 and x2 from x 114 | x2 = x(4:6,:); 115 | 116 | r = ... 117 | iscolinear(x1(:,1),x1(:,2),x1(:,3)) | ... 118 | iscolinear(x1(:,1),x1(:,2),x1(:,4)) | ... 119 | iscolinear(x1(:,1),x1(:,3),x1(:,4)) | ... 120 | iscolinear(x1(:,2),x1(:,3),x1(:,4)) | ... 121 | iscolinear(x2(:,1),x2(:,2),x2(:,3)) | ... 122 | iscolinear(x2(:,1),x2(:,2),x2(:,4)) | ... 123 | iscolinear(x2(:,1),x2(:,3),x2(:,4)) | ... 124 | iscolinear(x2(:,2),x2(:,3),x2(:,4)); 125 | 126 | function H = wrap_vgg_homography2d(x) 127 | 128 | xs1 = x(1:3,:); 129 | xs2 = x(4:6,:); 130 | H = vgg_H_from_x_lin(xs1,xs2); 131 | -------------------------------------------------------------------------------- /vgg_examples/testhomog_vgg.m: -------------------------------------------------------------------------------- 1 | % Demonstration of feature matching via simple correlation, and then using 2 | % RANSAC to estimate the homography matrix and at the same time identify 3 | % (mostly) inlying matches 4 | 5 | % Peter Kovesi 6 | % School of Computer Science & Software Engineering 7 | % The University of Western Australia 8 | % pk at csse uwa edu au 9 | % http://www.csse.uwa.edu.au/~pk 10 | % 11 | % February 2004 12 | 13 | % Adapted to use vgg functions by Peter Kovesi and Andrew Zisserman 14 | % 2012 Updated by Alexander Khanin 15 | 16 | function H = testhomog_vgg 17 | 18 | % prepare workspace 19 | close all 20 | clear all 21 | % clc 22 | 23 | % block of settings 24 | thresh = 100; % Harris corner threshold 25 | dmax = 100; % 26 | w = 11; % window size for correlation matching 27 | 28 | % try to read a pair of images 29 | try 30 | im1 = imread('keble.000.png'); 31 | im2 = imread('keble.003.png'); 32 | catch ME 33 | warning(ME.message); 34 | return; 35 | end 36 | 37 | % imput images must be grayscale 38 | if ndims(im1) == 3, im1 = rgb2gray(im1); end 39 | if ndims(im2) == 3, im2 = rgb2gray(im2); end 40 | 41 | % Find Harris corners in image1 and image2 42 | [cim1, r1, c1] = harris(im1, 1, thresh, 3); 43 | [cim2, r2, c2] = harris(im2, 1, thresh, 3); 44 | 45 | % create figure and fit it size to screen size 46 | scrsz = get(0,'ScreenSize'); 47 | figure('Name', 'Example', 'Position',[1 1 scrsz(3) scrsz(4)]) 48 | 49 | % display detected corners 50 | subplot(3,4,1),imshow(im1),title('Corners on 1st image'); 51 | hold on, plot(c1,r1,'r+'); 52 | subplot(3,4,5),imshow(im2),title('Corners on 2nd image'); 53 | hold on, plot(c2,r2,'r+'); 54 | 55 | % find correlation 56 | [m1,m2] = matchbycorrelation(im1, [r1';c1'], im2, [r2';c2'], w, dmax); 57 | 58 | % Display putative matches 59 | subplot(3,4,9),imshow(im1),title('Putative matches'); 60 | for n = 1:length(m1); 61 | line([m1(2,n) m2(2,n)], [m1(1,n) m2(1,n)]) 62 | end 63 | 64 | % Assemble homogeneous feature coordinates for fitting of the 65 | % homography matrix, note that [x,y] corresponds to [col, row] 66 | x1 = [m1(2,:); m1(1,:); ones(1,length(m1))]; 67 | x2 = [m2(2,:); m2(1,:); ones(1,length(m1))]; 68 | 69 | t = .001; % Distance threshold for deciding outliers 70 | [H, inliers] = ransacfithomography_vgg(x1, x2, t); 71 | 72 | fprintf('Number of inliers was %d (%d%%) \n', ... 73 | length(inliers),round(100*length(inliers)/length(m1))) 74 | fprintf('Number of putative matches was %d \n', length(m1)) 75 | 76 | % Display both images overlayed with inlying matched feature points 77 | subplot(3,4, [2 3 4 6 7 8 10 11 12]); 78 | imagesc(double(im1)+double(im2)), title('Inlying matches'), hold on 79 | plot(m1(2,inliers),m1(1,inliers),'r+'); 80 | plot(m2(2,inliers),m2(1,inliers),'g+'); 81 | 82 | % Step through each matched pair of points and display the 83 | % line linking the points on the overlayed images. 84 | for n = inliers 85 | line([m1(2,n) m2(2,n)], [m1(1,n) m2(1,n)],'color',[0 0 1]) 86 | end 87 | 88 | return 89 | end 90 | 91 | -------------------------------------------------------------------------------- /vgg_examples/testhomog_vgg_bak.m: -------------------------------------------------------------------------------- 1 | % Demonstration of feature matching via simple correlation, and then using 2 | % RANSAC to estimate the homography matrix and at the same time identify 3 | % (mostly) inlying matches 4 | 5 | % Peter Kovesi 6 | % School of Computer Science & Software Engineering 7 | % The University of Western Australia 8 | % pk at csse uwa edu au 9 | % http://www.csse.uwa.edu.au/~pk 10 | % 11 | % February 2004 12 | 13 | % Adapted to use vgg functions by Peter Kovesi and Andrew Zisserman 14 | 15 | function H = testhomog_vgg 16 | 17 | close all 18 | 19 | thresh = 500; % Harris corner threshold 20 | nonmaxrad = 3; % Non-maximal suppression radius 21 | dmax = 100; 22 | w = 11; % Window size for correlation matching 23 | 24 | im1 = rgb2gray(imread('keble.000.png')); 25 | im2 = rgb2gray(imread('keble.003.png')); 26 | 27 | % Find Harris corners in image1 and image2 28 | [cim1, r1, c1] = harris(im1, 1, thresh, 3); 29 | show(im1,1), hold on, plot(c1,r1,'r+'); 30 | 31 | [cim2, r2, c2] = harris(im2, 1, thresh, 3); 32 | show(im2,2), hold on, plot(c2,r2,'r+'); 33 | 34 | drawnow 35 | 36 | tic 37 | [m1,m2] = matchbycorrelation(im1, [r1';c1'], im2, [r2';c2'], w, dmax); 38 | toc 39 | % Display putative matches 40 | show(im1,3), set(3,'name','Putative matches'), hold on 41 | for n = 1:length(m1); 42 | line([m1(2,n) m2(2,n)], [m1(1,n) m2(1,n)]) 43 | end 44 | 45 | % Assemble homogeneous feature coordinates for fitting of the 46 | % homography matrix, note that [x,y] corresponds to [col, row] 47 | x1 = [m1(2,:); m1(1,:); ones(1,length(m1))]; 48 | x2 = [m2(2,:); m2(1,:); ones(1,length(m1))]; 49 | 50 | t = .001; % Distance threshold for deciding outliers 51 | [H, inliers] = ransacfithomography_vgg(x1, x2, t); 52 | 53 | fprintf('Number of inliers was %d (%d%%) \n', ... 54 | length(inliers),round(100*length(inliers)/length(m1))) 55 | fprintf('Number of putative matches was %d \n', length(m1)) 56 | 57 | % Display both images overlayed with inlying matched feature points 58 | show(double(im1)+double(im2),4), set(4,'name','Inlying matches'), hold on 59 | plot(m1(2,inliers),m1(1,inliers),'r+'); 60 | plot(m2(2,inliers),m2(1,inliers),'g+'); 61 | 62 | % Step through each matched pair of points and display the 63 | % line linking the points on the overlayed images. 64 | 65 | for n = inliers 66 | line([m1(2,n) m2(2,n)], [m1(1,n) m2(1,n)],'color',[0 0 1]) 67 | end 68 | 69 | return 70 | 71 | 72 | -------------------------------------------------------------------------------- /vgg_examples/view_fund_ex.m: -------------------------------------------------------------------------------- 1 | % Example of using vgg_gui_F 2 | 3 | % read in chapel images 4 | 5 | im1 = imread('chapel00.png'); 6 | im2 = imread('chapel01.png'); 7 | 8 | % read in fundamental matrix 9 | F = load('chapel.00.01.F'); 10 | 11 | % view epipolar geometry in GUI, move mouse with button down 12 | % in either window to see transferred point 13 | % NB function uses F transpose 14 | vgg_gui_F(im1, im2, F') 15 | -------------------------------------------------------------------------------- /vgg_examples/view_homog_ex.m: -------------------------------------------------------------------------------- 1 | % Example of using vgg_gui_H 2 | 3 | % read in basement images 4 | 5 | im1 = imread('bt.000.png'); 6 | im2 = imread('bt.002.png'); 7 | 8 | % read in homography for ground plane 9 | H = load('bt.00.02.H'); 10 | 11 | % view homography in GUI, move mouse with button down 12 | % in either window to see transferred point 13 | vgg_gui_H(im1, im2, H) 14 | -------------------------------------------------------------------------------- /vgg_general/vgg_argparse.m: -------------------------------------------------------------------------------- 1 | function [opts,rem_opts] = vgg_argparse(opts,varargin) 2 | 3 | %VGG_ARGPARSE Parse variable arguments into a structure 4 | % opts = vgg_argparse(inopts,varargin) 5 | % inopts: structure (cells array) listing valid members and default values 6 | % varargin: variable arguments of form '',,... 7 | % opts: opts modified by varargin 8 | % 9 | % Example: 10 | % function f = foo(varargin) 11 | % opts = vgg_argparse(struct('maxiters',10,'verbose',0), varargin) 12 | % ... 13 | % 14 | % An unknown option (ie, present in varargin but absent in inopts) 15 | % causes an error. Calling the function as 16 | % [opts,rem_opts] = vgg_argparse(inopts,varargin) returns the unknown 17 | % option(s) in rem_opts for later use rather than causes an error. 18 | % 19 | % May also use OPTS = VGG_ARGPARSE(OPTS, ASTRUCT) where ASTRUCT is a struct 20 | % of options. 21 | 22 | % Author: Mark Everingham 23 | % modified by werner, Jan 03 24 | % Date: 16 Jan 02 25 | 26 | if iscell(opts) 27 | opts=struct(opts{:}); 28 | end 29 | 30 | if length(varargin) & iscell(varargin{1}) 31 | if isempty(varargin{1}) 32 | inopts = struct([]); 33 | else 34 | inopts=struct(varargin{1}{:}); 35 | end 36 | else 37 | if isempty(varargin) 38 | inopts = struct([]); 39 | elseif isstruct(varargin{1}) 40 | inopts = varargin{1}; 41 | else 42 | inopts=struct(varargin{:}); 43 | end 44 | end 45 | 46 | rem_opts = []; 47 | fn = fieldnames(inopts); 48 | for i=1:length(fn) 49 | if isfield(opts,fn{i}) 50 | %opts.(fn{i})=inopts.(fn{i}); 51 | opts = setfield(opts,fn{i},getfield(inopts,fn{i})); 52 | else 53 | if nargout < 2 54 | error(sprintf('bad argument: ''%s''', fn{i})); 55 | else 56 | %rem_opts.(fn{i}) = inopts.(fn{i}); 57 | rem_opts = setfield(rem_opts,fn{i},getfield(inopts,fn{i})); 58 | end 59 | end 60 | end -------------------------------------------------------------------------------- /vgg_multiview/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jmmanley/VGG-Multiple-View-Geometry/f114712de03082bb97229eaf2a65981908b64127/vgg_multiview/.DS_Store -------------------------------------------------------------------------------- /vgg_multiview/Contents.m: -------------------------------------------------------------------------------- 1 | % VGG MultiView Compute Library 2 | % 3 | % Conversions 4 | % vgg_KR_from_P - extract K, R from P such that P = K*R*[eye(3) -t] 5 | % vgg_F_from_P - fundamental matrix from 2 cameras 6 | % vgg_P_from_F - 2 camera matrices from fundamental matrix 7 | % vgg_T_from_P - trifocal tensor from 3 cameras 8 | % vgg_H_from_2P_plane - inter-image homography from 2 cameras and 3D plane 9 | % vgg_H_from_P_plane - projection matrix from image onto 3D plane 10 | % vgg_plane_from_2P_H - 3D plane from 2 cameras and inter-image homography 11 | % 12 | % Multiview tensors from image correspondences 13 | % vgg_H_from_x_lin - homography from points in 2 images, linear method 14 | % vgg_H_from_x_nonlin - MLE of the above, by nonlinear method 15 | % vgg_Haffine_from_x_MLE - MLE of affine transformation from points in 2 images, linear 16 | % vgg_F_from_7pts_2img - fundamental matrix from 7 points in 2 images 17 | % vgg_PX_from_6pts_3img - cameras and world points from 6 points in 3 images 18 | % 19 | % Preconditioning for estimation 20 | % vgg_conditioner_from_image - conditioning shift+scaling from image dimensions 21 | % vgg_conditioner_from_pts - conditioning shift+scaling from image points 22 | % 23 | % Self-calibration and similar 24 | % vgg_signsPX_from_x - swaps signs of P and X so that projection scales are positive 25 | % vgg_selfcalib_qaffine - quasi-affine from projective reconstruction 26 | % vgg_selfcalib_metric_vansq - metric from projective and 3 orthogonal principal directions and square pixels 27 | % 28 | % Estimation 29 | % vgg_X_from_xP_lin - 3D point from image projections and cameras, linear 30 | % vgg_X_from_xP_nonlin - MLE of that, non-linear method 31 | % vgg_line3d_from_lP_lin - 3D line segment from image line segments and cameras, linear 32 | % vgg_line3d_from_lP_nonlin - MLE of that, non-linear method 33 | % 34 | % 3D lines representations 35 | % vgg_line3d_pv_from_XY - Pluecker vector from 2 points on the line 36 | % vgg_line3d_pv_from_pm - Pluecker matrix from Pluecker vector 37 | % vgg_line3d_pm_from_pv - Pluecker vector from Pluecker matrix 38 | % vgg_line3d_Ppv - rearrange camera matrix to project Pluecker vector to image line 39 | % vgg_line3d_pv_from_2planes - Pluecker vector from 2 planes meeting in the line 40 | % vgg_line3d_XY_from_pm - 2 points on 3D line from Pluecker matrix 41 | % vgg_line3d_XY_from_pv - 2 points on 3D line from Pluecker vector 42 | % (vgg_contreps - dual of Pluecker matrix of 3D line) 43 | % 44 | % Auxiliary & miscellaneous 45 | % vgg_get_homg - adding row of ones 46 | % vgg_get_nonhomg - dividing by the final coordinates 47 | % vgg_projective_basis_2d 48 | % vgg_rms_error 49 | % vgg_scatter_plot_homg 50 | % vgg_scatter_plot 51 | -------------------------------------------------------------------------------- /vgg_multiview/private/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jmmanley/VGG-Multiple-View-Geometry/f114712de03082bb97229eaf2a65981908b64127/vgg_multiview/private/.DS_Store -------------------------------------------------------------------------------- /vgg_multiview/private/CVS/Entries: -------------------------------------------------------------------------------- 1 | /vgg_H_sampson_distance_sqr.m/1.1/Mon Feb 18 19:08:16 2002// 2 | /vgg_condition_2d.m/1.1/Mon Feb 18 19:08:16 2002// 3 | /vgg_decondition_2d.m/1.1/Mon Feb 18 19:08:16 2002// 4 | /vgg_singF_from_FF.m/1.1/Tue Nov 12 17:50:34 2002// 5 | D 6 | -------------------------------------------------------------------------------- /vgg_multiview/private/CVS/Repository: -------------------------------------------------------------------------------- 1 | vgg_matlab/vgg_multiview/private 2 | -------------------------------------------------------------------------------- /vgg_multiview/private/CVS/Root: -------------------------------------------------------------------------------- 1 | az@angelo:/users/vgg/cvsroot 2 | -------------------------------------------------------------------------------- /vgg_multiview/private/vgg_H_sampson_distance_sqr.m: -------------------------------------------------------------------------------- 1 | function d = vgg_H_sampson_distance_sqr(H,X1,X2) 2 | 3 | % d = vgg_H_sampson_distance_sqr(H,X1,X2) 4 | % 5 | % Returns an approximation of the squared geometric distance from the 6 | % 4d joint-space point [X1;X2] to the H manifold. See Torr CVIU 97. 7 | 8 | if (size(X1) ~= size(X2)) 9 | error('Point sets not same size!'); 10 | end 11 | 12 | p1 = X1 ./ repmat(X1(3,:),3,1); 13 | p2 = X2 ./ repmat(X2(3,:),3,1); 14 | 15 | alg = vgg_H_algebraic_distance(H,p1,p2); 16 | 17 | N = size(X1,2); 18 | 19 | h = reshape(H',9,1); 20 | 21 | G1 = [ H(1,1) - p2(1,:) * H(3,1) ; ... 22 | H(1,2) - p2(1,:) * H(3,2) ; ... 23 | -p1(1,:) * H(3,1) - p1(2,:) * H(3,2) - H(3,3) ; ... 24 | zeros(1,N) ]; 25 | 26 | G2 = [ H(2,1) - p2(2,:) * H(3,1) ; ... 27 | H(2,2) - p2(2,:) * H(3,2) ; ... 28 | zeros(1,N) ; ... 29 | -p1(1,:) * H(3,1) - p1(2,:) * H(3,2) - H(3,3) ]; 30 | 31 | magG1 = sqrt(sum(G1 .* G1)); 32 | magG2 = sqrt(sum(G2 .* G2)); 33 | magG1G2 = sum(G1 .* G2); 34 | 35 | alpha = acos( magG1G2 ./ (magG1 .* magG2) ); 36 | 37 | D1 = alg(1,:) ./ magG1; 38 | D2 = alg(2,:) ./ magG2; 39 | 40 | d = (D1.*D1 + D2.*D2 - 2 * D1 .* D2 .* cos(alpha)) ./ sin(alpha); 41 | 42 | 43 | -------------------------------------------------------------------------------- /vgg_multiview/private/vgg_condition_2d.m: -------------------------------------------------------------------------------- 1 | function pc = vgg_condition_2d(p,C); 2 | % function pc = vgg_condition_2d(p,C); 3 | % 4 | % Condition a set of 2D homogeneous or nonhomogeneous points using conditioner C 5 | 6 | [r,c] = size(p); 7 | if r == 2 8 | pc = vgg_get_nonhomg(C * vgg_get_homg(p)); 9 | elseif r == 3 10 | pc = C * p; 11 | else 12 | error ('rows != 2 or 3'); 13 | end 14 | -------------------------------------------------------------------------------- /vgg_multiview/private/vgg_decondition_2d.m: -------------------------------------------------------------------------------- 1 | function pd = vgg_decondition_2d(p,C); 2 | % function pd = vgg_decondition_2d(p,C); 3 | % 4 | % Decondition a set of 2D homogeneous or nonhomogeneous points using (original) conditioner C 5 | 6 | [r,c] = size(p); 7 | if r == 2 8 | pd = vgg_get_nonhomg(inv(C) * vgg_get_homg(p)); 9 | elseif r == 3 10 | pd = inv(C) * p; 11 | else 12 | error ('rows != 2 or 3'); 13 | end 14 | -------------------------------------------------------------------------------- /vgg_multiview/private/vgg_singF_from_FF.m: -------------------------------------------------------------------------------- 1 | %VGG_SINGF_FROM_FF Linearly combines two 3x3 matrices to a singular one. 2 | % 3 | % a = vgg_singF_from_FF(F) computes scalar(s) a such that given two 3x3 matrices F{1} and F{2}, 4 | % it is det( a*F{1} + (1-a)*F{2} ) == 0. 5 | 6 | function a = vgg_singF_from_FF(F) 7 | 8 | % precompute determinants made from columns of F{1}, F{2} 9 | for i1 = 1:2 10 | for i2 = 1:2 11 | for i3 = 1:2 12 | D(i1,i2,i3) = det([F{i1}(:,1) F{i2}(:,2) F{i3}(:,3)]); 13 | end 14 | end 15 | end 16 | 17 | % Solve The cubic equation for a 18 | a = roots([-D(2,1,1)+D(1,2,2)+D(1,1,1)+D(2,2,1)+D(2,1,2)-D(1,2,1)-D(1,1,2)-D(2,2,2) 19 | D(1,1,2)-2*D(1,2,2)-2*D(2,1,2)+D(2,1,1)-2*D(2,2,1)+D(1,2,1)+3*D(2,2,2) 20 | D(2,2,1)+D(1,2,2)+D(2,1,2)-3*D(2,2,2) 21 | D(2,2,2)]); 22 | a = a(abs(imag(a))<10*eps); 23 | 24 | return -------------------------------------------------------------------------------- /vgg_multiview/vgg_F_from_7pts_2img.m: -------------------------------------------------------------------------------- 1 | %vgg_F_from_7pts_2img Computes fundamental matrix from 7 points across 2 images. 2 | % 3 | % [P,X] = vgg_F_from_7pts_2img(x), where 4 | % x ... double(3,7,2) or cell{2} of double(3,7), 7 homogeneous points in 2 images 5 | % F ... double(3,3), fundamental matrix 6 | % There are 0 to 3 solutions for F. Solutions are pruned by requirement that 7 | % scalars s in all equations s*cross(e1,x1)==F*x2 are positive. 8 | % In case of multiple solutions, F has one dimension 9 | % more such that F(:,:,n) is the n-th solution. 10 | % 11 | % Also the form F = vgg_F_from_7pts_2img(x1,x2) is accepted. 12 | 13 | function F = vgg_F_from_7pts_2img(x1,x2) 14 | 15 | if nargin==1 16 | if iscell(x1) 17 | x2 = x1{2}; 18 | x1 = x1{1}; 19 | else 20 | x2 = x1(:,:,2); 21 | x1 = x1(:,:,1); 22 | end 23 | end 24 | if any(size(x1)~=[3 7]) | any(size(x2)~=[3 7]) 25 | error('Wrong size of input points.'); 26 | end 27 | 28 | % Linear step 29 | A = vgg_vec_swap(x1,x2)'; 30 | [u,s,v] = svd(A,0); 31 | FF{1} = reshape(v(:,end-1),[3 3]); 32 | FF{2} = reshape(v(:,end ),[3 3]); 33 | 34 | % Solving cubic equation and getting 1 or 3 solutions for F 35 | a = vgg_singF_from_FF(FF); 36 | F = []; 37 | for i = 1:length(a) 38 | Fi = a(i)*FF{1} + (1-a(i))*FF{2}; 39 | %for n = 1:7, disp(norm(x(:,n,1)'*Fi*x(:,n,2))), end % test code 40 | if signs_OK(Fi,x1,x2) 41 | F = cat(3, F, Fi); 42 | end 43 | end 44 | 45 | return 46 | 47 | %%%%%%%%%%%%%%%%%%%%%%%%% 48 | 49 | % Checks sign consistence of F and x 50 | function OK = signs_OK(F,x1,x2) 51 | [u,s,v] = svd(F'); 52 | e1 = v(:,3); 53 | l1 = vgg_contreps(e1)*x1; 54 | s = sum( (F*x2) .* l1 ); 55 | OK = all(s>0) | all(s<0); 56 | return -------------------------------------------------------------------------------- /vgg_multiview/vgg_F_from_P.m: -------------------------------------------------------------------------------- 1 | %F = vgg_F_from_P(P) Compute fundamental matrix from two camera matrices. 2 | % P is cell (2), P = {P1 P2}. F has size (3,3). It is x2'*F*x1 = 0 3 | % 4 | % Overall scale of F is unique and such that, for any X, P1, P2, it is 5 | % F*x1 = vgg_contreps(e2)*x2, where 6 | % x1 = P1*X, x2 = P2*X, e2 = P2*C1, C1 = vgg_wedge(P1). 7 | 8 | function F = vgg_F_from_P(P, P2) 9 | 10 | if nargin == 1 11 | P1 = P{1}; 12 | P2 = P{2}; 13 | else 14 | P1 = P; 15 | end 16 | 17 | X1 = P1([2 3],:); 18 | X2 = P1([3 1],:); 19 | X3 = P1([1 2],:); 20 | Y1 = P2([2 3],:); 21 | Y2 = P2([3 1],:); 22 | Y3 = P2([1 2],:); 23 | 24 | F = [det([X1; Y1]) det([X2; Y1]) det([X3; Y1]) 25 | det([X1; Y2]) det([X2; Y2]) det([X3; Y2]) 26 | det([X1; Y3]) det([X2; Y3]) det([X3; Y3])]; 27 | 28 | return -------------------------------------------------------------------------------- /vgg_multiview/vgg_H_algebraic_distance.m: -------------------------------------------------------------------------------- 1 | function d = vgg_H_algebraic_distance(H,X1,X2) 2 | 3 | % d = vgg_H_algebraic_distance(H,X1,X2) 4 | % 5 | % For sets of homg points X1 and X2, returns the algebraic distances 6 | % d = (p2'_x p2'_y) * p1_w - (p1_x p1_y) * p2'_w 7 | 8 | if (size(X1) ~= size(X2)) 9 | error('Point sets not same size!'); 10 | end 11 | 12 | N = size(X1,2); 13 | 14 | Dx = [ X1' .* repmat(X2(3,:)',1,3) , zeros(N,3) , -X1' .* repmat(X2(1,:)',1,3) ]; 15 | 16 | Dy = [ zeros(N,3) , X1' .* repmat(X2(3,:)',1,3) , -X1' .* repmat(X2(2,:)',1,3) ]; 17 | 18 | h = reshape(H',9,1); 19 | 20 | d = [Dx * h , Dy * h]'; -------------------------------------------------------------------------------- /vgg_multiview/vgg_H_from_2P_plane.m: -------------------------------------------------------------------------------- 1 | function [H] = vgg_H_from_2P_plane(P1,P2, L) 2 | % 3 | % [H] = vgg_H_from_2P_plane(P, L) 4 | % [H] = vgg_H_from_2P_plane(P1, P2, L) 5 | % 6 | %USAGE 1: 7 | % Returns the homography matrix from the plane whos normal is L to the 8 | % image whos projection matrix is P. This is useful for mapping two images 9 | % of a plane. 10 | %USAGE 2: 11 | % Given two camera matrices and a plane, returns the homography matrix that 12 | % maps points from the first camera onto the second. 13 | % 14 | %IN: 15 | % P,P1,P2 - 3x4 Camera matrix 16 | % L - 1x4 Plane normal 17 | % 18 | %OUT: 19 | % H - 3x3 projective homography matrix mapping points from the image 20 | % to the world-points on the plane. Note that 21 | % 22 | %EXAMPLE: 23 | % Let P1, P2 be two projection matrices and let L be the normal to 24 | % the plane then the homography H=H1*inv(H2) maps points in P2 to 25 | % points in P1 where: 26 | % H1=vgg_H_from_2P_plane(P1, L) 27 | % H2=vgg_H_from_2P_plane(P2, L) 28 | % % And this will be the resulting image: 29 | % [u,v]=homflow(H, size(i1,1), size(i1,2)); 30 | % t=imgwarp(i1, u,v); 31 | % 32 | % An alternative is to use H = P1*vgg_H_from_P_plane(P2,L). 33 | 34 | % $Id: vgg_H_from_2P_plane.m,v 1.2 2002/02/22 22:30:55 werner Exp $ 35 | % Yoni, Fri Apr 6 12:44:54 2001 36 | 37 | if nargin==2 38 | P=P1; L=P2; 39 | 40 | H = P(:,1:3)*L(4) - P(:,4)*L(1:3); 41 | 42 | elseif nargin==3 43 | nL=null(L); 44 | H=P2*nL*inv(P1*nL); 45 | 46 | else 47 | error('Wrong number of arguments'); 48 | end 49 | -------------------------------------------------------------------------------- /vgg_multiview/vgg_H_from_P_plane.m: -------------------------------------------------------------------------------- 1 | % vgg_H_from_P_plane Given 3D plane and camera, returns 4-by-3 matrix mapping image points to points on the plane. 2 | % 3 | % H = vgg_H_from_P_plane(A,P), where 4 | % A ... size (4,1), scene plane 5 | % P ... size (3,4), camera matrix 6 | % H ... size (4,3), matrix such that X = x*H, where x (size (3,1)) is an image point 7 | % and X is a scene point lying on the scene plane A. 8 | % In other words, P*H = s*eye(3), where s is a scalar factor. 9 | % 10 | % The overall sign of H is chosen such that s*A*vgg_wedge(P) > 0. 11 | 12 | % T. Werner, Oct 2001 13 | 14 | function H = vgg_H_from_P_plane(A,P) 15 | 16 | A = A'; 17 | p1 = P(1,:); 18 | p2 = P(2,:); 19 | p3 = P(3,:); 20 | 21 | H = [vgg_contreps(p2'*p3 - p3'*p2)*A ... 22 | vgg_contreps(p3'*p1 - p1'*p3)*A ... 23 | vgg_contreps(p1'*p2 - p2'*p1)*A]; 24 | 25 | return 26 | -------------------------------------------------------------------------------- /vgg_multiview/vgg_H_from_x_lin.m: -------------------------------------------------------------------------------- 1 | function H = vgg_H_from_x_lin(xs1,xs2) 2 | % H = vgg_H_from_x_lin(xs1,xs2) 3 | % 4 | % Compute H using linear method (see Hartley & Zisserman Alg 3.2 page 92 in 5 | % 1st edition, Alg 4.2 page 109 in 2nd edition). 6 | % Point preconditioning is inside the function. 7 | % 8 | % The format of the xs [p1 p2 p3 ... pn], where each p is a 2 or 3 9 | % element column vector. 10 | 11 | [r,c] = size(xs1); 12 | 13 | if (size(xs1) ~= size(xs2)) 14 | error ('Input point sets are different sizes!') 15 | end 16 | 17 | if (size(xs1,1) == 2) 18 | xs1 = [xs1 ; ones(1,size(xs1,2))]; 19 | xs2 = [xs2 ; ones(1,size(xs2,2))]; 20 | end 21 | 22 | % condition points 23 | C1 = vgg_conditioner_from_pts(xs1); 24 | C2 = vgg_conditioner_from_pts(xs2); 25 | xs1 = vgg_condition_2d(xs1,C1); 26 | xs2 = vgg_condition_2d(xs2,C2); 27 | 28 | D = []; 29 | ooo = zeros(1,3); 30 | for k=1:c 31 | p1 = xs1(:,k); 32 | p2 = xs2(:,k); 33 | D = [ D; 34 | p1'*p2(3) ooo -p1'*p2(1) 35 | ooo p1'*p2(3) -p1'*p2(2) 36 | ]; 37 | end 38 | 39 | % Extract nullspace 40 | [u,s,v] = svd(D, 0); s = diag(s); 41 | 42 | nullspace_dimension = sum(s < eps * s(1) * 1e3); 43 | if nullspace_dimension > 1 44 | fprintf('Nullspace is a bit roomy...'); 45 | end 46 | 47 | h = v(:,9); 48 | 49 | H = reshape(h,3,3)'; 50 | 51 | %decondition 52 | H = inv(C2) * H * C1; 53 | 54 | H = H/H(3,3); 55 | -------------------------------------------------------------------------------- /vgg_multiview/vgg_H_from_x_nonlin.m: -------------------------------------------------------------------------------- 1 | function [H,rms] = vgg_H_from_x_nonlin(H_initial,p1,p2) 2 | 3 | % [H,rms] = vgg_H_from_x_nonlin(H_initial,xs1,xs2) 4 | % 5 | % Compute H using non-linear method which minimizes Sampson's approx to 6 | % geometric reprojection error (see Hartley & Zisserman Alg 3.3 page 98 in 7 | % 1st edition, Alg 4.3 page 114 in 2nd edition). 8 | 9 | % An initial estimate of 10 | % H is required, which would usually be obtained using 11 | % vgg_H_from_x_linear. It is not necessary to precondition the 12 | % supplied points. 13 | % 14 | % The format of the xs is 15 | % [x1 x2 x3 ... xn ; 16 | % y1 y2 y3 ... yn ; 17 | % w1 w2 w3 ... wn] 18 | 19 | [r,c] = size(p1); 20 | 21 | if (size(p1) ~= size(p2)) 22 | error ('Input point sets are different sizes!') 23 | end 24 | 25 | global gp1 gp2 C1 C2; 26 | 27 | gp1 = p1; 28 | gp2 = p2; 29 | 30 | % Make conditioners 31 | C1 = vgg_conditioner_from_pts(p1); 32 | C2 = vgg_conditioner_from_pts(p2); 33 | 34 | H_cond = C2 * H_initial * inv(C1); 35 | 36 | % the following is added for compatibility with both Matlab R2012a and earlier versions 37 | % Thank you to Peter Corke for this update 38 | 39 | if verLessThan('matlab', '7.10') 40 | opt = optimset( optimset('lsqnonlin') , 'LargeScale','off', 'Diagnostics','off', 'Display','off'); 41 | else 42 | opt = optimset( optimset('lsqnonlin') , 'Algorithm','levenberg-marquardt', 'Diagnostics','off', 'Display','off'); 43 | end 44 | 45 | % opt = optimset( optimset('lsqnonlin') , 'LargeScale','off', 'Diagnostics','off', 'Display','off'); 46 | 47 | H_cond = lsqnonlin(@lsq_func,H_cond,[],[],opt); 48 | 49 | H = inv(C2) * H_cond * C1; 50 | rms = sqrt(vgg_H_sampson_distance_sqr(H,gp1,gp2)); 51 | 52 | clear gp1 gp2 C1 C2; 53 | 54 | 55 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 56 | 57 | function r = lsq_func(H) 58 | global gp1 gp2 C1 C2; 59 | 60 | H_decond = inv(C2) * H * C1; 61 | 62 | d = vgg_H_sampson_distance_sqr(H_decond,gp1,gp2); 63 | 64 | r = sqrt(d); 65 | 66 | -------------------------------------------------------------------------------- /vgg_multiview/vgg_H_sampson_distance_sqr.m: -------------------------------------------------------------------------------- 1 | function d = vgg_H_sampson_distance_sqr(H,X1,X2) 2 | 3 | % d = vgg_H_sampson_distance_sqr(H,X1,X2) 4 | % 5 | % Returns an approximation of the squared geometric distance from the 6 | % 4d joint-space point [X1;X2] to the H manifold. See Torr CVIU 97. 7 | 8 | if (size(X1) ~= size(X2)) 9 | error('Point sets not same size!'); 10 | end 11 | 12 | p1 = X1 ./ repmat(X1(3,:),3,1); 13 | p2 = X2 ./ repmat(X2(3,:),3,1); 14 | 15 | alg = vgg_H_algebraic_distance(H,p1,p2); 16 | 17 | N = size(X1,2); 18 | 19 | h = reshape(H',9,1); 20 | 21 | G1 = [ H(1,1) - p2(1,:) * H(3,1) ; ... 22 | H(1,2) - p2(1,:) * H(3,2) ; ... 23 | -p1(1,:) * H(3,1) - p1(2,:) * H(3,2) - H(3,3) ; ... 24 | zeros(1,N) ]; 25 | 26 | G2 = [ H(2,1) - p2(2,:) * H(3,1) ; ... 27 | H(2,2) - p2(2,:) * H(3,2) ; ... 28 | zeros(1,N) ; ... 29 | -p1(1,:) * H(3,1) - p1(2,:) * H(3,2) - H(3,3) ]; 30 | 31 | magG1 = sqrt(sum(G1 .* G1)); 32 | magG2 = sqrt(sum(G2 .* G2)); 33 | magG1G2 = sum(G1 .* G2); 34 | 35 | alpha = acos( magG1G2 ./ (magG1 .* magG2) ); 36 | 37 | D1 = alg(1,:) ./ magG1; 38 | D2 = alg(2,:) ./ magG2; 39 | 40 | d = (D1.*D1 + D2.*D2 - 2 * D1 .* D2 .* cos(alpha)) ./ sin(alpha); 41 | 42 | 43 | -------------------------------------------------------------------------------- /vgg_multiview/vgg_Haffine_from_x_MLE.m: -------------------------------------------------------------------------------- 1 | function H = vgg_Haffine_from_x_MLE(xs1,xs2) 2 | % H = vgg_Haffine_from_x_MLE(xs1,xs2) 3 | % 4 | % Compute MLE for affine H, i.e. find H and xhat1 such that 5 | % d^2(xs1,xhat1) + d^2(xs2,xhat2) minimized where xhat2 is affine transf of xhat1. 6 | % 7 | % Parameters: 8 | % xs1, xs2 ... double(3,N), N pairs of corresponding points (homogeneous) 9 | % H ... double(3,3), affine transformation 10 | % 11 | % See HZ page 115 1st edition, page 130 2nd edition 12 | % az 17/11/2001 13 | 14 | if any(size(xs1) ~= size(xs2)) 15 | error ('Input point sets are different sizes!') 16 | end 17 | 18 | % condition points 19 | 20 | nonhomg = vgg_get_nonhomg(xs1); 21 | means = mean(nonhomg'); 22 | maxstds = max(std(nonhomg')); 23 | C1 = diag([1/maxstds 1/maxstds 1]); % only similarity 24 | C1(:,3) = [-means/maxstds 1]'; 25 | 26 | nonhomg = vgg_get_nonhomg(xs2); 27 | means = mean(nonhomg'); 28 | C2 = C1; % nb must use same scaling for both point sets 29 | C2(:,3) = [-means/maxstds 1]'; 30 | 31 | xs1 = vgg_condition_2d(xs1,C1); 32 | xs2 = vgg_condition_2d(xs2,C2); 33 | 34 | % NB conditioned points have mean zero, so translation 35 | % part of affine transf is zero 2-vector 36 | 37 | xs1nh = vgg_get_nonhomg(xs1); 38 | xs2nh = vgg_get_nonhomg(xs2); 39 | 40 | A = [xs1nh;xs2nh]'; 41 | 42 | % Extract nullspace 43 | [u,s,v] = svd(A); 44 | s = diag(s); 45 | 46 | nullspace_dimension = sum(s < eps * s(2) * 1e3); 47 | if nullspace_dimension > 2 48 | fprintf('Nullspace is a bit roomy...'); 49 | end 50 | 51 | % compute affine matrix from two largest singular vecs 52 | 53 | B = v(1:2,1:2); 54 | C = v(3:4,1:2); 55 | 56 | H = [ C * pinv(B) , zeros(2,1); 0 0 1]; 57 | 58 | %decondition 59 | H = inv(C2) * H * C1; 60 | 61 | H = H/H(3,3); 62 | -------------------------------------------------------------------------------- /vgg_multiview/vgg_KR_from_P.m: -------------------------------------------------------------------------------- 1 | %VGG_KR_FROM_P Extract K, R from camera matrix. 2 | % 3 | % [K,R,t] = VGG_KR_FROM_P(P [,noscale]) finds K, R, t such that P = K*R*[eye(3) -t]. 4 | % It is det(R)==1. 5 | % K is scaled so that K(3,3)==1 and K(1,1)>0. Optional parameter noscale prevents this. 6 | % 7 | % Works also generally for any P of size N-by-(N+1). 8 | % Works also for P of size N-by-N, then t is not computed. 9 | 10 | 11 | % Author: Andrew Fitzgibbon 12 | % Modified by werner. 13 | % Date: 15 May 98 14 | 15 | 16 | function [K, R, t] = vgg_KR_from_P(P, noscale) 17 | 18 | N = size(P,1); 19 | H = P(:,1:N); 20 | 21 | [K,R] = vgg_rq(H); 22 | 23 | if nargin < 2 24 | K = K / K(N,N); 25 | if K(1,1) < 0 26 | D = diag([-1 -1 ones(1,N-2)]); 27 | K = K * D; 28 | R = D * R; 29 | 30 | % test = K*R; 31 | % vgg_assert0(test/test(1,1) - H/H(1,1), 1e-07) 32 | end 33 | end 34 | 35 | if nargout > 2 36 | t = -P(:,1:N)\P(:,end); 37 | end 38 | 39 | return -------------------------------------------------------------------------------- /vgg_multiview/vgg_PX_from_6pts_3img.m: -------------------------------------------------------------------------------- 1 | function [P,X] = vgg_PX_from_6pts_3img(x1,x2,x3) 2 | % vgg_PX_from_6pts_3img Computes camera matrices and world points 3 | % from 6 points across 3 images. 4 | % 5 | % [P,X] = vgg_PX_from_6pts_3img(x), where 6 | % x ... double(3,6,3) or cell{3} of double(3,6), 6 homogeneous points in 3 images 7 | % P ... double(3,4,3), P(:,:,k) is k-th camera matrix 8 | % X ... double(4,6), homogeneous world points 9 | % There are 0 to 3 solutions for (P,X). Solutions are pruned by requirement that 10 | % scalars s in all projective equations s*x==P*X are positive. 11 | % In case of multiple solutions, P and X have one dimension 12 | % more such that P(:,:,:,n) and X(:,:,n) is the n-th solution. 13 | % 14 | % Also the form [P,X] = vgg_PX_from_6pts_3img(x1,x2,x3) is accepted. 15 | 16 | % Algorithm in Hartley-Zisserman, Alg 19.1 page 493 in 1st edition, 17 | % Alg 20.1 page 511 in 2nd edition 18 | % Coded by werner@robots.ox.ac.uk, Nov 2002. 19 | 20 | if nargin==3 21 | x = cat(3,x1,x2,x3); 22 | else 23 | if iscell(x1) 24 | x = cat(3,x1{:}); 25 | else 26 | x = x1; 27 | end 28 | end 29 | if any(size(x)~=[3 6 3]) 30 | error('Wrong size of input points.'); 31 | end 32 | 33 | for k = 1:3 34 | % Find homographies H_k mapping first 4 pts in each image to standard projective basis. 35 | % Now, x(:,1:4,k) = H(:,:,k)*[eye(3) [1;1;1]]. 36 | H(:,:,k) = H_from_4x(x(:,3:6,k)); 37 | 38 | % Form transformed points xs(:,1:2,k) 39 | xs(:,:,k) = inv(H(:,:,k))*x(:,:,k); 40 | end 41 | 42 | % Compute dual fundamental matrix 43 | Fd = Fdual_from_x(xs(:,1:2,:)); 44 | 45 | % Retrieve (non-dual) cameras P and world points X from (each solution for) dual fund. matrix. 46 | P = []; 47 | X = []; 48 | for i = 1:size(Fd,3) 49 | 50 | % Compute canonical Xi and Pi 51 | Xi = X_from_Fdual(Fd(:,:,i)); 52 | Pi = P_from_Xx_canonical(Xi,xs); 53 | 54 | % return from canonical to original image bases 55 | for k = 1:3 56 | Pi(:,:,k) = H(:,:,k)*Pi(:,:,k); 57 | end 58 | 59 | % Compute signs of P and X, if possible. If impossible, Pi==Xi==[]. 60 | [Pi,Xi] = vgg_signsPX_from_x(Pi,Xi,x); 61 | if isempty(Pi), continue, end 62 | %for k = 1:3, Pi(:,:,k)*Xi ./ x(:,:,k), end % test code 63 | 64 | P = cat(4,P,Pi); 65 | X = cat(3,X,Xi); 66 | 67 | end 68 | 69 | return 70 | 71 | 72 | %%%%%%%%%%%%%%%% auxiliary functions 73 | 74 | 75 | % Solve for dual fundamental matrix. 76 | function F = Fdual_from_x(x) 77 | 78 | % Linear step. After that, 79 | % for k=1:3 and i=1:2, it is x(:,1,k)'*F_i*x(:,2,k)==0. 80 | A = []; 81 | for k = 1:3 82 | x1 = x(1,1,k); y1 = x(2,1,k); z1 = x(3,1,k); 83 | x2 = x(1,2,k); y2 = x(2,2,k); z2 = x(3,2,k); 84 | A = [A; [x1*y2-z1*y2,... 85 | x1*z2-z1*y2,... 86 | y1*x2-z1*y2,... 87 | y1*z2-z1*y2,... 88 | z1*x2-z1*y2] ]; 89 | end 90 | [u,s,v] = svd(A,0); 91 | v1 = v(:,end-1); 92 | v2 = v(:,end); 93 | FF{1} = [0 v1(1:2)'; v1(3) 0 v1(4); v1(5) -sum(v1) 0]; 94 | FF{2} = [0 v2(1:2)'; v2(3) 0 v2(4); v2(5) -sum(v2) 0]; 95 | 96 | % Non-linear step. Find linear combination of F_i having zero determinant. 97 | % Dual fund. matrix is now F = a*FF{1} + (1-a)*FF{2}, for each element of a. 98 | a = vgg_singF_from_FF(FF); 99 | for i = 1:length(a) 100 | F(:,:,i) = a(i)*FF{1} + (1-a(i))*FF{2}; 101 | end 102 | return 103 | 104 | 105 | % Given dual fundamental matrix Fd, computes cameras P and world points X such that 106 | % for each k, P(:,:,k)*X ~ x(:,:,k). 107 | function X = X_from_Fdual(Fd) 108 | 109 | % Retrieve second dual camera matrix Pd2. 110 | % It is done by considering Fdual in form 111 | %[0 b*(d-c) -c*(d-b) 112 | % -a*(d-c) 0 c*(d-a) 113 | % a*(d-b) -b*(d-a) 0]. 114 | 115 | Fd_aux = reshape( Fd([4 7 1 2 1 8 1 3 6]), [3 3] ); 116 | [u,s,v] = svd(Fd_aux,0); 117 | ABC = v(:,3); % It is a:b:c = A:B:C 118 | [u,s,v] = svd(Fd',0); 119 | KLM = v(:,3); % It is K:L:M = ((d-a):(d-b):(d-c) 120 | 121 | G = [0 -ABC(3) ABC(2) 0 122 | ABC(3) 0 -ABC(1) 0 123 | -ABC(2) ABC(1) 0 0 124 | KLM(2) -KLM(1) 0 KLM(1)-KLM(2) 125 | 0 KLM(3) -KLM(2) KLM(2)-KLM(3) 126 | -KLM(3) 0 KLM(1) KLM(3)-KLM(1) ]; 127 | [u,s,v] = svd(G,0); 128 | abcd = v(:,end); 129 | 130 | % The test code: 131 | %Pd1 = [eye(3) [1;1;1]]; 132 | %Pd2 = [diag(abcd(1:3)) [1;1;1]*abcd(4)]; 133 | %vgg_F_from_P(Pd1,Pd2) ./ Fd 134 | 135 | % Retrieve 6 world points X(:,1:6) 136 | X = [ abcd [1;1;1;1] eye(4) ]; 137 | 138 | return 139 | 140 | 141 | % Computes camera P from world points X and image points x, everything in canonical form. 142 | function P = P_from_Xx_canonical(X,x) 143 | X = X(:,1); 144 | for k = 1:3 145 | A = [contreps(x(:,1,k))*[X(1) 0 0 X(4) 146 | 0 X(2) 0 X(4) 147 | 0 0 X(3) X(4)] 148 | contreps(x(:,2,k))*[eye(3) [1;1;1]]]; 149 | [u,s,v] = svd(A,0); 150 | a = v(:,end); 151 | P(:,:,k) = [a(1) 0 0 a(4) 152 | 0 a(2) 0 a(4) 153 | 0 0 a(3) a(4)]; 154 | end 155 | return 156 | 157 | 158 | % A bit faster vgg_contreps. 159 | function X = contreps(x) 160 | X = [0 x(3) -x(2) 161 | -x(3) 0 x(1) 162 | x(2) -x(1) 0]; 163 | return 164 | 165 | 166 | % H_from_4x Having four point matches, the homography relating them is given by 167 | % H = H_from_4x(x2)*inv(H_from_4x(x1)). 168 | function H = H_from_4x(x) 169 | H = x(:,1:3) * diag(inv(x(:,1:3))*x(:,4)); 170 | return 171 | 172 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 173 | 174 | 175 | % Test code: 176 | P = randn(3,4,3); X = randn(4,6); 177 | for k = 1:3, x(:,:,k) = P(:,:,k)*X; end 178 | [Pn,Xn] = vgg_PX_from_6pts_3img(x); 179 | 180 | % check image points x predicted up to scale 181 | for k=1:3, Pn(:,:,k)*Xn(:,:) ./ x(:,:,k), end 182 | 183 | -------------------------------------------------------------------------------- /vgg_multiview/vgg_P_from_F.m: -------------------------------------------------------------------------------- 1 | %P = vgg_P_from_F(F) Compute cameras from fundamental matrix. 2 | % F has size (3,3), P has size (3,4). 3 | % 4 | % If x2'*F*x1 = 0 for any pair of image points x1 and x2, 5 | % then the camera matrices of the image pair are 6 | % P1 = eye(3,4) and P2 = vgg_P_from_F(F), up to a scene homography. 7 | 8 | % Tomas Werner, Oct 2001 9 | 10 | function P = vgg_P_from_F(F) 11 | 12 | [U,S,V] = svd(F); 13 | e = U(:,3); 14 | P = [-vgg_contreps(e)*F e]; 15 | 16 | return -------------------------------------------------------------------------------- /vgg_multiview/vgg_T_from_P.m: -------------------------------------------------------------------------------- 1 | % T = vgg_T_from_P(P) Trifocal tensor from 3 camera matrices. 2 | % 3 | % P ... cell (3), camera matrices 4 | % T ... double (3,3,3) 5 | % 6 | % For 3 corresponding lines l1..l3 (each of size (1,3)) in cameras P1..P3 it is 7 | % for i=1:3, l1(1,i) = l2*T(:,:,i)*l3'; end 8 | % up to scale. 9 | % 10 | % T is obtained by with unique absolute scale. In detail, for any cameras P{1:3} and 11 | % scene line L (4-by-4 Pluecker matrix, L+L'=0, vgg_contreps(L)*L=0), if 12 | % 13 | % for i=1:3, l{i} = vgg_contreps(P{i}*L*P{i}'); end 14 | % T = vgg_T_from_P(P); 15 | % for i=1:3, l1(1,i) = l{2}*T(:,:,i)*l{3}'; end 16 | % 17 | % then 18 | % 19 | % l1 == l{1}*(l{3}*e32) == -l{1}*(l{2}*e23) 20 | % 21 | % where e32 = P{3}*vgg_C_from_P(P{2}), e23 = P{2}*vgg_C_from_P(P{3}) are epipoles. 22 | 23 | function T = vgg_T_from_P(P) 24 | 25 | T(:,:,1) = P{2}*vgg_contreps(P{1}(2,:)'*P{1}(3,:)-P{1}(3,:)'*P{1}(2,:))*P{3}'; 26 | T(:,:,2) = P{2}*vgg_contreps(P{1}(3,:)'*P{1}(1,:)-P{1}(1,:)'*P{1}(3,:))*P{3}'; 27 | T(:,:,3) = P{2}*vgg_contreps(P{1}(1,:)'*P{1}(2,:)-P{1}(2,:)'*P{1}(1,:))*P{3}'; 28 | 29 | return -------------------------------------------------------------------------------- /vgg_multiview/vgg_X_from_xP_lin.m: -------------------------------------------------------------------------------- 1 | %vgg_X_from_xP_lin Estimation of 3D point from image matches and camera matrices, linear. 2 | % X = vgg_X_from_xP_lin(x,P,imsize) computes projective 3D point X (column 4-vector) 3 | % from its projections in K images x (2-by-K matrix) and camera matrices P (K-cell 4 | % of 3-by-4 matrices). Image sizes imsize (2-by-K matrix) are needed for preconditioning. 5 | % By minimizing algebraic distance. 6 | % 7 | % See also vgg_X_from_xP_nonlin. 8 | 9 | % werner@robots.ox.ac.uk, 2003 10 | 11 | function X = vgg_X_from_xP_lin(u,P,imsize) 12 | 13 | if iscell(P) 14 | P = cat(3,P{:}); 15 | end 16 | K = size(P,3); 17 | 18 | if nargin>2 19 | for k = 1:K 20 | H = [2/imsize(1,k) 0 -1 21 | 0 2/imsize(2,k) -1 22 | 0 0 1]; 23 | P(:,:,k) = H*P(:,:,k); 24 | u(:,k) = H(1:2,1:2)*u(:,k) + H(1:2,3); 25 | end 26 | end 27 | 28 | A = []; 29 | for k = 1:K 30 | A = [A; vgg_contreps([u(:,k);1])*P(:,:,k)]; 31 | end 32 | % A = normx(A')'; 33 | [dummy,dummy,X] = svd(A,0); 34 | X = X(:,end); 35 | 36 | % Get orientation right 37 | s = reshape(P(3,:,:),[4 K])'*X; 38 | if any(s<0) 39 | X = -X; 40 | if any(s>0) 41 | % warning('Inconsistent orientation of point match'); 42 | end 43 | end 44 | 45 | return 46 | -------------------------------------------------------------------------------- /vgg_multiview/vgg_X_from_xP_nonlin.m: -------------------------------------------------------------------------------- 1 | %vgg_X_from_xP_nonlin Estimation of 3D point from image matches and camera matrices, nonlinear. 2 | % X = vgg_X_from_xP_lin(x,P,imsize) computes max. likelihood estimate of projective 3 | % 3D point X (column 4-vector) from its projections in K images x (2-by-K matrix) 4 | % and camera matrices P (K-cell of 3-by-4 matrices). Image sizes imsize (2-by-K matrix) 5 | % are needed for preconditioning. 6 | % By minimizing reprojection error, Newton iterations. 7 | % 8 | % X = vgg_X_from_xP_lin(x,P,imsize,X0) takes initial estimate of X. If X0 is omitted, 9 | % it is computed by linear algorithm. 10 | % 11 | % See also vgg_X_from_xP_lin. 12 | 13 | % werner@robots.ox.ac.uk, 2003 14 | 15 | function X = vgg_X_from_xP_nonlin(u,P,imsize,X) 16 | 17 | if iscell(P) 18 | P = cat(3,P{:}); 19 | end 20 | K = size(P,3); 21 | if K < 2 22 | error('Cannot reconstruct 3D from 1 image'); 23 | end 24 | 25 | if nargin==3 26 | X = vgg_X_from_xP_lin(u,P,imsize); 27 | end 28 | 29 | if nargin==2 30 | X = vgg_X_from_xP_lin(u,P); 31 | end 32 | 33 | % precondition 34 | if nargin>2 35 | for k = 1:K 36 | H = [2/imsize(1,k) 0 -1 37 | 0 2/imsize(2,k) -1 38 | 0 0 1]; 39 | P(:,:,k) = H*P(:,:,k); 40 | u(:,k) = H(1:2,1:2)*u(:,k) + H(1:2,3); 41 | end 42 | end 43 | 44 | % Parametrize X such that X = T*[Y;1]; thus x = P*T*[Y;1] = Q*[Y;1] 45 | [dummy,dummy,T] = svd(X',0); 46 | T = T(:,[2:end 1]); 47 | for k = 1:K 48 | Q(:,:,k) = P(:,:,k)*T; 49 | end 50 | 51 | % Newton 52 | Y = [0;0;0]; 53 | eprev = inf; 54 | for n = 1:10 55 | [e,J] = resid(Y,u,Q); 56 | if 1-norm(e)/norm(eprev) < 1000*eps 57 | break 58 | end 59 | eprev = e; 60 | Y = Y - (J'*J)\(J'*e); 61 | end 62 | 63 | X = T*[Y;1]; 64 | 65 | return 66 | 67 | %%%%%%%%%%%%%%%%%%%%%%%%%% 68 | 69 | function [e,J] = resid(Y,u,Q) 70 | K = size(Q,3); 71 | e = []; 72 | J = []; 73 | for k = 1:K 74 | q = Q(:,1:3,k); 75 | x0 = Q(:,4,k); 76 | x = q*Y + x0; 77 | e = [e; x(1:2)/x(3)-u(:,k)]; 78 | J = [J; [x(3)*q(1,:)-x(1)*q(3,:) 79 | x(3)*q(2,:)-x(2)*q(3,:)]/x(3)^2]; 80 | end 81 | return 82 | -------------------------------------------------------------------------------- /vgg_multiview/vgg_conditioner_from_image.m: -------------------------------------------------------------------------------- 1 | function [C,invC] = vgg_conditioner_from_image(c,r) 2 | % 3 | % function [C,invC] = vgg_conditioner_from_image(image_width, image_height) 4 | % 5 | % Makes a similarity metric for conditioning image points. 6 | % 7 | % Also can be called as vgg_conditioner_from_image([image_width image_height]) 8 | % 9 | % invC is inv(C), obtained more efficiently inside the function. 10 | 11 | if nargin<2 12 | r = c(2); 13 | c = c(1); 14 | end 15 | 16 | f = (c+r)/2; 17 | C = [1/f 0 -c/(2*f) ; 18 | 0 1/f -r/(2*f) ; 19 | 0 0 1]; 20 | 21 | if nargout > 1 22 | invC = [f 0 c/2 ; 23 | 0 f r/2 ; 24 | 0 0 1]; 25 | end -------------------------------------------------------------------------------- /vgg_multiview/vgg_conditioner_from_pts.m: -------------------------------------------------------------------------------- 1 | function T = vgg_conditioner_from_pts(Pts,isotropic) 2 | 3 | % VGG_CONDITIONER_FROM_PTS - Returns a conditioning matrix for points 4 | % 5 | % T = vgg_conditioner_from_pts(Pts [,isotropic]) 6 | % 7 | % Returns a DxD matrix that normalizes Pts to have mean 0 and stddev sqrt(2) 8 | % 9 | % 10 | %IN: 11 | % Pts - DxK list of K projective points. Last row is ignored. 12 | % isotropic - optional; if present then T(1,1)==T(2,2)==...==T(D-1,D-1). 13 | % 14 | % 15 | %OUT: 16 | % T - DxD conditioning matrix 17 | 18 | % Yoni, Thu Feb 14 12:24:48 2002 19 | 20 | Dim=size(Pts,1); 21 | 22 | Pts=vgg_get_nonhomg(Pts); 23 | Pts=Pts(1:Dim-1,:); 24 | 25 | m=mean(Pts,2); 26 | s=std(Pts'); 27 | s=s+(s==0); 28 | 29 | if nargin==1 30 | T=[ diag(sqrt(2)./s) -diag(sqrt(2)./s)*m]; 31 | else % isotropic; added by TW 32 | T=[ diag(sqrt(2)./(ones(1,Dim-1)*mean(s))) -diag(sqrt(2)./s)*m]; 33 | end 34 | T(Dim,:)=0; 35 | T(Dim,Dim)=1; 36 | -------------------------------------------------------------------------------- /vgg_multiview/vgg_fit_hplane_to_x.m: -------------------------------------------------------------------------------- 1 | % vgg_fit_hplane_to_x Fitting hyperplane to set of points. 2 | % 3 | % SYNOPSIS 4 | % [A,e] = vgg_fit_hplane_to_x(s), where 5 | % 6 | % s ... double(N+1,N+1), inv. covariance matrix s = x'*x, the columns of which 7 | % are (N+1)-vectors of homog. coordinates of points in N-space (s(end,:)==1) 8 | % A ... double(1,N+1), fitted hyperplane (homog. coordinates). 9 | % Homogeneous part of A is normalized, norm(A(1,1:N))==1. 10 | % e ... double(N,1), eignevalues of the fit 11 | % 12 | % Hyperplane A minimizes sum of squared orthogonal distances of points to it. 13 | % 14 | % EXAMPLE 15 | % Let e be (2,?) vector of detected edgels in nonhomog. coords. Then fitting a straight 16 | % line l to this set of edgels is: 17 | % e(end,:) = 1; 18 | % l = vgg_fit_hplane_to_x(e*e'); 19 | 20 | function [A,e] = vgg_fit_hplane_to_x(s) 21 | 22 | N = size(s,1) - 1; % space dimension 23 | c = s(1:N,end)/s(end,end); % centroid 24 | 25 | [U,e,V] = svd(s(1:N,1:N)-c*s(end,1:N),0); 26 | 27 | A = U(:,end)'; 28 | A = [A -A*c]; 29 | e = diag(e); 30 | 31 | return 32 | -------------------------------------------------------------------------------- /vgg_multiview/vgg_get_homg.m: -------------------------------------------------------------------------------- 1 | function x = vgg_get_homg(x) 2 | % h = vgg_get_homg(p) 3 | % 4 | % Convert a set of non-homogeneous points into homogeneous points 5 | % Points are stored as column vectors, stacked horizontally, e.g. 6 | % [x0 x1 x2 x3 ... xn ; 7 | % y0 y1 y2 y3 ... yn] 8 | 9 | % Modified by TW 10 | 11 | if isempty(x) 12 | return 13 | end 14 | 15 | x(end+1,:) = 1; 16 | 17 | return 18 | -------------------------------------------------------------------------------- /vgg_multiview/vgg_get_nonhomg.m: -------------------------------------------------------------------------------- 1 | function x = vgg_get_nonhomg(x) 2 | % p = vgg_get_nonhomg(h) 3 | % 4 | % Convert a set of homogeneous points to non-homogeneous form 5 | % Points are stored as column vectors, stacked horizontally, e.g. 6 | % [x0 x1 x2 ... xn ; 7 | % y0 y1 y2 ... yn ; 8 | % w0 w1 w2 ... wn ] 9 | 10 | % Modified by TW 11 | 12 | if isempty(x) 13 | x = []; 14 | return; 15 | end 16 | 17 | d = size(x,1) - 1; 18 | x = x(1:d,:)./(ones(d,1)*x(end,:)); 19 | 20 | return -------------------------------------------------------------------------------- /vgg_multiview/vgg_line3d_Ppv.m: -------------------------------------------------------------------------------- 1 | % Q = vgg_line3d_Ppv(P) Transforms a camera matrix to use it with Pluecker vector 3d line representation. 2 | % 3 | % P ... double(3,4), ordinary camera matrix 4 | % Q ... double(6,3), transformed camera matrix (quadratic function of elements of P) 5 | % such that the projection of a 3d line by the camera is given by 6 | % 7 | % l = Q*L 8 | % 9 | % where l is 1-by-3 image line vector, and L is 1-by-6 Pluecker vector of the 3d line. 10 | 11 | % T.Werner 12 | 13 | function Q = vgg_line3d_Ppv(P) 14 | 15 | K = size(P,1)/3; 16 | i1 = 1:3:3*K; 17 | i2 = 2:3:3*K; 18 | i3 = 3:3:3*K; 19 | 20 | Q(:,i1) = vgg_line3d_pv_from_XY(P(i2,:)',P(i3,:)')'; 21 | Q(:,i2) = vgg_line3d_pv_from_XY(P(i3,:)',P(i1,:)')'; 22 | Q(:,i3) = vgg_line3d_pv_from_XY(P(i1,:)',P(i2,:)')'; 23 | 24 | return -------------------------------------------------------------------------------- /vgg_multiview/vgg_line3d_XY_from_pm.m: -------------------------------------------------------------------------------- 1 | % XY = vgg_line3d_XY_from_pm(L) Converts Pluecker matrix 3d line to a pair of homogeneous 3d points. 2 | % 3 | % L ... double(4,4), skew-symmetric Pluecker matrix of 3d line 4 | % XY ... double(4,2), pair of 3d points spanning the line, in homogen. coordinates 5 | % 6 | % XY are obtained by svd, their homogeneous vectors are mutually orthogonal. 7 | 8 | % T.Werner 9 | 10 | function XY = vgg_line3d_XY_from_pm(L) 11 | 12 | [u,s,v] = svd(vgg_contreps(L),0); 13 | XY = u(:,3:4); 14 | 15 | return -------------------------------------------------------------------------------- /vgg_multiview/vgg_line3d_XY_from_pv.m: -------------------------------------------------------------------------------- 1 | % XY = vgg_line3d_XY_from_pv(L) Converts Pluecker vector 3d line to a pair of homogeneous points. 2 | % 3 | % L ... double(1,6), Pluecker vector 4 | % XY ... double(4,2), pair of 3d points spanning the line 5 | % 6 | % XY are obtained by svd, their homogeneous vectors are mutually orthogonal. 7 | 8 | % T.Werner 9 | 10 | function XY = vgg_line3d_XY_from_pv(L) 11 | 12 | XY = vgg_line3d_XY_from_pm(vgg_line3d_pm_from_pv(L)); 13 | 14 | return -------------------------------------------------------------------------------- /vgg_multiview/vgg_line3d_from_lP_lin.m: -------------------------------------------------------------------------------- 1 | % vgg_line3d_linear Linear estimation of 3d line from image lines and camera matrices. 2 | % 3 | % SYNOPSIS 4 | % L = vgg_line3d_from_lP_lin(s,P [,imsize]), where 5 | % 6 | % s ... cell(K) of double(3,3), inv. covariance matrices of the K image line segments:- 7 | % - If the segments are estimated from edges, it is s(:,k) = x*x', 8 | % where x (3-by-N) are homog. coordinates of the edgels with last components 1. 9 | % - If only end points are available, s(:,k) = d*x*y' where x, y (column 2-vectors) 10 | % are the segment's end points and d its length. 11 | % 12 | % P ... cell(K) of double(3,4), camera matrices 13 | % 14 | % imsize ... double(2,K), image sizes (for preconditioning). 15 | % Omit if s and P are already preconditioned. 16 | % 17 | % L ... double(4,2), 3d straight line; columns of L are two homogeneous 18 | % points spanning the line. 19 | 20 | function L = vgg_line3d_from_lP_lin(s,P,imsize) 21 | 22 | if nargin < 3, imsize = []; end 23 | 24 | K = length(P); % number of images 25 | 26 | % l := straight lines in homog. coords 27 | for k = 1:K 28 | l(k,:) = vgg_fit_hplane_to_x(s{k}); 29 | end 30 | 31 | if ~isempty(imsize) & K>2 % Preconditioning; for 2 views is not needed 32 | for k = 1:K 33 | [H,invH] = vgg_conditioner_from_image(imsize(:,k)); 34 | P{k} = H*P{k}; 35 | l(k,:) = l(k,:)*invH; 36 | end 37 | l = norml(l); 38 | end 39 | 40 | M = []; 41 | for k = 1:K 42 | M = [M; l(k,:)*P{k}]; 43 | end 44 | 45 | [u,s,v] = svd(normx(M')',0); 46 | L = v(:,end-1:end); 47 | 48 | return 49 | 50 | 51 | function x = normx(x) 52 | x = x./(ones(size(x,1),1)*sqrt(sum(x.*x))); 53 | 54 | function l = norml(l) 55 | % l = norml(l) Multiplies hyperplane l by scalar so that for each n, norm(l(1:end-1,n))==1. 56 | l = l./(sqrt(sum(l(:,1:end-1).^2,2))*ones(1,size(l,2))); 57 | -------------------------------------------------------------------------------- /vgg_multiview/vgg_line3d_from_lP_nonlin.m: -------------------------------------------------------------------------------- 1 | % vgg_line3d_from_lP_nonlin Non-linear estimation of (possibly constrained) 3D line segment from image line segments. 2 | % 3 | % SYNOPSIS 4 | % L = vgg_line3d_from_lP_nonlin(s,P [,imsize] [,L0] [,X] [,nonlin_opt]) 5 | % 6 | % s ... cell(K) of double(3,3), inv. covariance matrices of the K image line segments:- 7 | % - If the segments are estimated from edges, it is s(:,k) = x*x', 8 | % where x (3-by-N) are homog. coordinates of the edgels with last components 1. 9 | % - If only end points are available, s(:,k) = d*x*y' where x, y (column 2-vectors) 10 | % are the segment's end points and d its length. 11 | % 12 | % P ... K-cell with 3-by-4 camera matrices 13 | % 14 | % imsize ... size (2,K), image size(s) for preconditiong. 15 | % Omit if s and P are already preconditioned. 16 | % 17 | % L0 ... double(4,2), initial scene line (optional). Homogeneous points L0(:,i) span 18 | % the line. If omitted, linear estimation is done first. 19 | % 20 | % X ... constraint on L :- 21 | % - if X is omitted: no constraint on L 22 | % - if X is double(4,1): L goes thru point X 23 | % - if X is double(4,2): L goes thru 3D line spanned by X(:,i) 24 | % 25 | % nonlin_opt ... options for Levenberg-Marquardt. It is comma-separated list 26 | % of pairs ['option',value]. Possible options are :- 27 | % opt ... options structure with possible fields :- 28 | % - verbose ... 1 or 0 (default: 0) 29 | % - niter_term ... maximum number of iterations 30 | % - rmsstep_term ... terminating step of rms of residuals 31 | % - lambda_term ... terminating value of lambda (default: 1e10) 32 | % - lambda_init ... initial value of lambda 33 | % E.g., vgg_line3d_from_lP_nonlin(...,'lambda_init',1e-9,'niter_term',5). 34 | % 35 | % L ... double(4,2), estimated 3D line. Points L(:,i) span the line. 36 | % 37 | % Note: use [] if you want to omit a parameter and use a later one, e.g. 38 | % vgg_line3d_from_lP_nonlin(s,P,imsize,[],[],'verbose',1,'lam_init',1e-9) 39 | % 40 | % ALGORITHM 41 | % - Minimization is done by Levenberg-Marquardt. 42 | % - 3D line L is parameterized by image lines in the first two images. 43 | % The positions of these image lines are possibly constrained by X. 44 | 45 | % T.Werner, Feb 2002 46 | 47 | function L = vgg_line3d_from_lP_nonlin(s,P,imsize,L,X,varargin) 48 | 49 | if nargin < 3, imsize = []; end 50 | if nargin < 4, L = []; end 51 | if nargin < 5, X = []; end 52 | 53 | if isempty(L) 54 | L = vgg_line3d_from_lP_lin(s,P,imsize); 55 | end 56 | K = length(P); % number of images 57 | if K<2 58 | error('Cannot reconstruct 3D line from 1 image'); 59 | end 60 | if isempty(X) & K==2 % no need for non-linear minimization 61 | return 62 | end 63 | 64 | % Prepare square root of covariance matrices; now s{k}(:,n) has meaning of 3 homogeneous image points 65 | for k = 1:K 66 | [us,ss,vs] = svd(s{k},0); 67 | s{k} = us*sqrt(ss); 68 | end 69 | 70 | % Preconditioning 71 | if ~isempty(imsize) 72 | for k = 1:K 73 | H = vgg_conditioner_from_image(imsize(:,k)); 74 | P{k} = H*P{k}; 75 | s{k} = H*s{k}; 76 | scale(k) = H(1,1); % save the scales for evaluating objective function 77 | end 78 | else 79 | scale = ones(1,K); 80 | end 81 | 82 | switch size(X,2) 83 | case 0 84 | % Scene line L is unconstrained, having thus 4 DOF. 85 | % L is parameterized by two image lines in images 1 and 2, each having 2 DOF, as follows: 86 | % l1 = l0(1,:) + p(1:2)'*ldelta{1} 87 | % l2 = l0(2,:) + p(3:4)'*ldelta{2} 88 | % where row 4-vector p represents 4 DOF of L. 89 | for k = 1:2 90 | l0(k,:) = normx(vgg_wedge(P{k}*L)')'; 91 | ldelta{k} = null(l0(k,:))'; 92 | end 93 | 94 | % optimization 95 | p = levmarq(@F, {vertcat(P{:}),s,scale,l0,ldelta},... 96 | @normsolve,... 97 | [0;0;0;0],... 98 | varargin{:}); 99 | l = l12_from_p(p,l0,ldelta); 100 | 101 | case 1 102 | % Scene line L is constrained to intersect the scene point X, having thus 2 DOF. 103 | % L is parameterized by two image lines in images 1 and 2, each having 1 DOF, as follows: 104 | % l1 = l0(1,:) + p(1)*ldelta{1} 105 | % l2 = l0(2,:) + p(2)*ldelta{2} 106 | % where 2-vector p represents 2 DOF of L. 107 | for k = 1:2 108 | l0(k,:) = normx(vgg_wedge(P{k}*L)')'; 109 | x = P{k}*X; 110 | 111 | % Since L might not intersect X, move l0(k,:) 'as little as possible' to intersect x. 112 | l0(k,:) = l0(k,:) - (l0(k,:)*x)/(x'*x).*x'; 113 | 114 | Q = null(x')'; 115 | ldelta{k} = null(l0(k,:)*pinv(Q))'*Q; 116 | end 117 | 118 | % optimization 119 | p = levmarq(@F, {vertcat(P{:}),s,scale,l0,ldelta},... 120 | @normsolve,... 121 | [0;0],... 122 | varargin{:}); 123 | l = l12_from_p(p,l0,ldelta); 124 | 125 | case 2 126 | % Scene line L is constrained to intersect the scene line given by 2 points X. 127 | % This constraint is given by 128 | % l1*G*l2' = 0 129 | % where G is 3x3 rank 2 matrix (analogical in fact to fundamental matrix) and 130 | % G = P{1}*M*P{2}' 131 | % where M is Pluecker matrix of line given by X, M = X(:,1)*X(:,2)'-X(:,2)*X(:,1). 132 | % 133 | % This constraint can be written as 134 | % p(1:2)*D*p(3:4)' + p(1:2)*d{2}' + d{1}*p(3:4)' + c = 0 135 | % where D, d, c are given below and 4-vector p are 4 parameters of L. 136 | % 137 | % L is parameterized by two image lines in images 1 and 2, each having 2 DOF, as follows: 138 | % l1 = l0(1,:) + p(1:2)'*ldelta{1} 139 | % l2 = l0(2,:) + p(3:4)'*ldelta{2} 140 | % where p(1:3) are chosen freely and p(4) is computed from the above formula as 141 | % p(4) = -(p(1:2)'*(D(:,1)*p(3)+d{1})+p(3)*d{2}(1)+c)/(p(1:2)'*D(:,2)+d{2}(2)). 142 | Lpm = X(:,1)*X(:,2)' - X(:,2)*X(:,1)'; 143 | G = P{1}*Lpm*P{2}'; 144 | for k = 1:2 145 | l0(k,:) = normx(vgg_wedge(P{k}*L)')'; 146 | end 147 | 148 | % As L might not intersect line X, move l0(2,:) 'as little as possible' to enforce l0(1,:)*G*l0(2,:)'==0. 149 | x = (l0(1,:)*G)'; 150 | l0(2,:) = l0(2,:) - (l0(2,:)*x)/(x'*x).*x'; 151 | 152 | for k = 1:2 153 | ldelta{k} = null(l0(k,:))'; 154 | end 155 | 156 | D = ldelta{1}*G*ldelta{2}'; 157 | d{1} = ldelta{1}*G*l0(2,:)'; 158 | d{2} = ldelta{2}*G'*l0(1,:)'; 159 | c = l0(1,:)*G*l0(2,:)'; 160 | 161 | % optimization 162 | p = levmarq(@F, {vertcat(P{:}),s,scale,l0,ldelta,D,d,c},... 163 | @normsolve,... 164 | [0;0;0],... 165 | varargin{:}); 166 | l = l12_from_p(p,l0,ldelta,D,d,c); 167 | 168 | end 169 | if all(~isnan(l(:))) 170 | L = null([l(1,:)*P{1}; l(2,:)*P{2}]); 171 | end 172 | 173 | return 174 | 175 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% objective function 176 | 177 | 178 | % Objective function of Levenberg-Marquardt 179 | function [y,w,J] = F(p,P,s,scale,varargin) 180 | K = length(s); 181 | 182 | % l := lines in images 1, 2 from p 183 | l = l12_from_p(p,varargin{:}); 184 | 185 | % l := reprojection of lines in images 1, 2 to all images 186 | if all(abs(p) < inf) 187 | [dummy,dummy,L]= svd([l(1,:)*P(1:3,:); l(2,:)*P(4:6,:)],0); 188 | else 189 | L = inf*ones(4); 190 | end 191 | l = norml(vgg_wedge(reshape(P*L(:,3),[3 K]),reshape(P*L(:,4),[3 K]))); 192 | 193 | % compute residual function 194 | y = []; 195 | for k = 1:K 196 | y = [y l(k,:)*s{k}]; 197 | end 198 | y = y'; 199 | 200 | w = [1;1;1] * (1./scale); 201 | w = w(:); 202 | 203 | % else, compute also jacobian 204 | if nargout < 2 205 | return 206 | end 207 | dif = 1e-6; 208 | J = zeros(length(y),length(p)); 209 | for i = 1:length(p) 210 | pdif = p; 211 | pdif(i) = pdif(i) + dif; 212 | J(:,i) = (F(pdif,P,s,scale,varargin{:}) - y)/dif; 213 | end 214 | return 215 | 216 | 217 | % The following function computes lines in the first two images 218 | % from parameters p. Explanation see above. 219 | function l = l12_from_p(p,l0,ldelta,D,d,c) 220 | switch length(p) 221 | case 4 % unconstrained 222 | l = [l0(1,:) + p(1:2)'*ldelta{1} 223 | l0(2,:) + p(3:4)'*ldelta{2}]; 224 | case 2 % going thru X 225 | l = [l0(1,:) + p(1)*ldelta{1} 226 | l0(2,:) + p(2)*ldelta{2}]; 227 | case 3 % going thru L 228 | p(4) = -(p(1:2)'*(D(:,1)*p(3)+d{1})+p(3)*d{2}(1)+c)/(p(1:2)'*D(:,2)+d{2}(2)); 229 | l = [l0(1,:) + p(1:2)'*ldelta{1} 230 | l0(2,:) + p(3:4)'*ldelta{2}]; 231 | end 232 | return 233 | 234 | 235 | function dp = normsolve(J,Y,w,lambda) 236 | OLDWARN = warning('off'); 237 | dp = -( J'*diag(w)*J + lambda*eye(size(J,2)) ) \ ( J'*(Y.*w) ); 238 | warning(OLDWARN); 239 | return 240 | 241 | function x = normx(x) 242 | if ~isempty(x) 243 | x = x./(ones(size(x,1),1)*sqrt(sum(x.*x))); 244 | end 245 | 246 | function l = norml(l) 247 | % l = norml(l) Multiplies hyperplane l by scalar so that for each n, norm(l(1:end-1,n))==1. 248 | l = l./(sqrt(sum(l(:,1:end-1).^2,2))*ones(1,size(l,2))); 249 | 250 | 251 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 252 | 253 | % a = levmarq(@RES,PARAMS,@NORMSOLVE,a [,opt]) Non-linear least-squares by Levenberg-Marquardt. 254 | % 255 | % Minimizes f(a)'*W*f(a) over a. 256 | % 257 | % @RES ... residual function f called like [e,w,J] = RES(a,PARAMS{:}), where 258 | % - a ... double(M,1), parameter vector 259 | % - e ... double(N,1), residual vector 260 | % - J ... double(N,M), derivative of e wrt a 261 | % - w ... double(N,1), weights of e; covariance matrix of e is diag(1/e.^2). 262 | % Use 1 instead of ones(N,1). 263 | % For efficiency, RES should not compute the jacobian if called with two output parameters only. 264 | % 265 | % @NORMSOLVE ... function solving normal equations, called like da = NORMSOLVE(J,e,W,lambda). 266 | % a ... initial parameter vector 267 | % opt ... options structure, see code 268 | 269 | function [a,w] = levmarq(RES,PARAMS,NORMSOLVE,a0,varargin) 270 | 271 | % options 272 | [opt,rem_opt] = vgg_argparse( { 'niter_term', +inf,... 273 | 'drmsrel_term', 0,... 274 | 'loglambda_term', 6,... 275 | 'loglambda_init', -4,... 276 | 'verbose', 0 },... 277 | varargin ); 278 | if ~isempty(rem_opt), if ~isempty(fieldnames(rem_opt)) 279 | error(['Unknown option(s) ' fieldnames(rem_opt)]); 280 | end, end 281 | 282 | 283 | % Initial statistics 284 | if opt.verbose 285 | [e0,w0] = feval(RES,a0,PARAMS{:}); 286 | ssd0 = sum( (e0.*w0).^2 ); 287 | fprintf( ' [rms=%14.12g] [maxabs=%14.12g]\n',... 288 | sqrt(ssd0/length(e0)),... 289 | max(abs(e0.*w0)) ); 290 | end 291 | 292 | 293 | loglambda = opt.loglambda_init; 294 | niter = 0; 295 | while 1 296 | 297 | % Compute actual residual and jacobian 298 | [e0,w0,J] = feval(RES,a0,PARAMS{:}); 299 | 300 | % Update a as a := a0 + da, by finding 301 | % optimal lambda and solving normal equations for da. 302 | nfail = 1; 303 | while (loglambda < opt.loglambda_term) 304 | niter = niter + 1; 305 | 306 | a = a0 + feval(NORMSOLVE,J,e0,w0,10^loglambda); 307 | [e,w] = feval(RES,a,PARAMS{:}); 308 | 309 | if sum((e.*w).^2) < sum((e0.*w0).^2) % success 310 | a0 = a; 311 | loglambda = loglambda - 1; 312 | break 313 | end 314 | 315 | if opt.verbose 316 | fprintf('%4i.%.2i: [loglambda=%3i] [REJECTED]\n',niter,nfail,loglambda); 317 | end 318 | 319 | loglambda = loglambda + 1; 320 | nfail = nfail + 1; 321 | end 322 | 323 | % Print statistic after successful iteration 324 | ssd0 = sum( (e0.*w0).^2 ); 325 | ssd = sum( (e.*w).^2 ); 326 | if opt.verbose 327 | fprintf( '%4i : [loglambda=%3i] [rms=%14.12g] [maxabs=%14.12g] [drmsrel=%4g%%]\n',... 328 | niter,... 329 | round(loglambda),... 330 | sqrt(ssd/length(e)),... 331 | max(abs(e.*w)),... 332 | 100*(1-sqrt(ssd/ssd0)) ); 333 | end 334 | 335 | % Termination criteria 336 | test(1) = loglambda < opt.loglambda_term; 337 | test(2) = ssd0-ssd >= opt.drmsrel_term^2*ssd0; 338 | test(3) = niter < opt.niter_term; 339 | if any(test==0) 340 | break 341 | end 342 | end 343 | 344 | if opt.verbose 345 | onoff = {'YES','no'}; 346 | fprintf( ' Levenberg-Marquardt finished succesfully.\n Reason for termination:\n' ); 347 | fprintf( ' lambda = %s\n', onoff{test(1)+1} ); 348 | fprintf( ' drmsrel = %s\n', onoff{test(2)+1} ); 349 | fprintf( ' niter = %s\n', onoff{test(3)+1} ); 350 | end 351 | 352 | return 353 | 354 | 355 | 356 | function print_statistics(niter,loglambda,e0,w0,e,w,opt) 357 | if opt.verbose 358 | ssd0 = sum( (e0.*w0).^2 ); 359 | ssd = sum( (e.*w).^2 ); 360 | fprintf( '%4i : [loglambda=%3i] [rms=%14.12g] [maxabs=%14.12g] [drmsrel=%11.5g]\n',... 361 | niter,... 362 | round(loglambda),... 363 | sqrt(ssd/length(e)),... 364 | max(abs(e.*w)),... 365 | sqrt(1-ssd/ssd0) ); 366 | end 367 | return 368 | 369 | -------------------------------------------------------------------------------- /vgg_multiview/vgg_line3d_pm_from_pv.m: -------------------------------------------------------------------------------- 1 | % G = vgg_line3d_pm_from_pv(L) Conversion of Pluecker vector to Puecker matrix 3d line representation. 2 | % 3 | % L ... double(1,6), Pluecker vector of the 3d line 4 | % G ... double(4,4), Puecker matrix 5 | 6 | % T.Werner 7 | 8 | function G = vgg_line3d_pm_from_pv(L) 9 | 10 | G = [vgg_contreps(L(4:6)) L(1:3)'; -L(1:3) 0]; 11 | 12 | return -------------------------------------------------------------------------------- /vgg_multiview/vgg_line3d_pv_from_2planes.m: -------------------------------------------------------------------------------- 1 | % L = vgg_line3d_pv_from_2planes(A,B) Pluecker vector of 3d line met by two planes. 2 | % 3 | % Syntax: L = vgg_line3d_pv_from_2planes(A,B) or 4 | % L = vgg_line3d_pv_from_2planes(AB) 5 | % 6 | % A, B ... size (N,4), 3d planes 7 | % AB ... size (2*N,4), N pairs of 3d planes. It is AB = [A1; B1; A2; B2; ... ; AN; BN]. 8 | % L ... size (N,6), line(s) in Pluecker vector. 9 | % 10 | % It is vectorized for N>1. 11 | 12 | % T.Werner 13 | 14 | function L = vgg_line3d_pv_from_2planes(A,B) 15 | 16 | if nargin == 1 17 | L = vgg_line3d_pv_from_XY(A'); 18 | else 19 | L = vgg_line3d_pv_from_XY(A',B'); 20 | end 21 | L = L(:,[4:6 1:3]); 22 | 23 | return -------------------------------------------------------------------------------- /vgg_multiview/vgg_line3d_pv_from_XY.m: -------------------------------------------------------------------------------- 1 | % L = vgg_line3d_pv_from_XY(X,Y) Pluecker vector 3d line from two 3d points. 2 | % 3 | % Syntax: L = vgg_line3d_pv_from_XY(X,Y) or 4 | % L = vgg_line3d_pv_from_XY(XY) 5 | % 6 | % X, Y ... size (4,N), 3D points 7 | % XY ... size (4,2*N), XY stacked as XY = [X1 Y1 ... XN YN]. 8 | % L ... size (N,6), Pluecker vector(s) of 3D line(s). 9 | % 10 | % It is vectorized for N>1. 11 | % 12 | % See vgg_line3d_* 13 | 14 | % T.Werner 15 | 16 | function L = vgg_line3d_pv_from_XY(X,Y) 17 | 18 | if nargin == 1 19 | Y = X(:,2:2:end); 20 | X = X(:,1:2:end); 21 | end 22 | 23 | if size(X,2) == 1 24 | L = [(X(1:3)*Y(4)-Y(1:3)*X(4))' vgg_wedge([X(1:3) Y(1:3)])]; 25 | else % vectorized version 26 | L = [(X(1:3,:).*([1;1;1]*Y(4,:))-Y(1:3,:).*([1;1;1]*X(4,:)))' vgg_wedge(X(1:3,:),Y(1:3,:))]; 27 | end 28 | 29 | return -------------------------------------------------------------------------------- /vgg_multiview/vgg_line3d_pv_from_pm.m: -------------------------------------------------------------------------------- 1 | % L = vgg_line3d_pv_from_pm(G) Conversion of Pluecker matrix to Pluecker vector 3d line representation. 2 | % 3 | % G ... double(4,4), skew-symmetric Pluecker matrix 4 | % L ... double(1,6), Pluecker vector 5 | 6 | % T.Werner 7 | 8 | function L = vgg_line3d_pv_from_pm(G) 9 | 10 | L = [G(1:3,4); vgg_contreps(G(1:3,1:3))']'; 11 | 12 | return -------------------------------------------------------------------------------- /vgg_multiview/vgg_plane_from_2P_H.m: -------------------------------------------------------------------------------- 1 | % A = vgg_plane_from_2P_H(P1,P2,H) From two cameras and inter-image homography, it gets the scene plane. 2 | % 3 | % The method is linear and there is no preconditioning - works only for consistent triplet (P1,P2,H). 4 | % 5 | % P1, P2 ... double (3,4) 6 | % H ... double (3,3) 7 | % A ... double (4,1) 8 | % 9 | % It is H = P2*vgg_H_from_2P_plane(A,P1). Try the example at the end of this file. 10 | 11 | function A = vgg_plane_from_2P_H(P,P2,H) 12 | 13 | Q = [P2*vgg_contreps(P(2,:)'*P(3,:) - P(3,:)'*P(2,:)) 14 | P2*vgg_contreps(P(3,:)'*P(1,:) - P(1,:)'*P(3,:)) 15 | P2*vgg_contreps(P(1,:)'*P(2,:) - P(2,:)'*P(1,:))]; 16 | A = (Q\H(:))'; 17 | 18 | return 19 | 20 | 21 | P = randn(3,4); 22 | P2 = randn(3,4); 23 | A = randn(1,4); 24 | H = P2*vgg_H_from_P_plane(A,P); 25 | vgg_plane_from_2P_H(P,P2,H) ./ A 26 | 27 | -------------------------------------------------------------------------------- /vgg_multiview/vgg_poly3d_orthorectify.m: -------------------------------------------------------------------------------- 1 | %vgg_poly3d_orthorectify Frontoparallel rectification of image of 3D polygon. 2 | % [H,imsize] = vgg_poly3d_orthorectify(Q,u) finds homography H that 3 | % removes perspective distortion of image Q*hom(u) of a 3D polygon. Parameters: 4 | % u ... double(2,N), inhomog. coordinates of polygon vertices measured in an orthonormal 5 | % coordinate frame in the 3D polygon plane. 6 | % Q ... double(3,3), homography that maps u to image plane. 7 | % Image of the polygon is thus x = Q*hom(u) (homog. coords.). 8 | % H ... double(3,3), rectifying homography 9 | % imsize ... double(2,1), size of the target image 10 | % The rectification is done as follows: 11 | % Irect = vgg_warp_H(I{k},inv(H),'linear',[1 imsize(2) 1 imsize(1)]). 12 | % 13 | % [H,imsize] = vgg_poly3d_orthorectify(Q,u,smax) allows to specify maximum ratio of 14 | % area element in output and input image (if omitted, smax=5). This is to prevent 15 | % the output image from becoming huge if the perspective distortion is large. 16 | % 17 | % If Q is K-cell of double(3,3), rectification is done simultaneously for K images 18 | % of the 3D polygon, so that pixels with the same coordinates correspond in the 19 | % rectified images. Then H is K-cell of double(3,3) and imsize is double(2,K). 20 | % 21 | % [H,imsize] = vgg_poly3d_orthorectify(Q,u,smax,in_imsize) specifies sizes of input 22 | % images. This is needed if some vertices project outside some input images. 23 | 24 | function [H,imsize] = vgg_poly3d_frontorectify(H,u,smax,imsize) 25 | 26 | if ~iscell(H) 27 | H = {H}; 28 | end 29 | 30 | if nargin<3 31 | smax = 5; 32 | end 33 | 34 | % find scales of H in all u 35 | for k = 1:length(H) 36 | if nargin>3 % consider only vertices inside input image 37 | i = boxclipu( [[1;1] imsize(:,k)], vgg_get_nonhomg(H{k}*vgg_get_homg(u)) ); 38 | else 39 | i = logical(ones(1,size(u,2))); 40 | end 41 | s(k,~i) = nan; 42 | if nnz(i)==0, continue, end 43 | s(k,i) = sqrt(abs( dethomog(H{k},u(:,i)) )); 44 | end 45 | 46 | % determine the scale; 47 | % forbid scaling polygons more than smax-times 48 | s = s(:); 49 | s(isnan(s)) = []; 50 | if isempty(s) % polygon is not in the image 51 | imsize = [0;0]; 52 | return 53 | end 54 | if nargin < 5 55 | smax = inf; 56 | end 57 | s = min(s) * min(smax,max(s)/min(s)); 58 | 59 | % scale H and u consistently by s 60 | u = u*s; 61 | Hs = diag([1/s 1/s 1]); 62 | for k = 1:length(H) 63 | H{k} = H{k}*Hs; 64 | end 65 | 66 | % translate the polygon to the origin 67 | b = bboxu(u); 68 | imsize = ceil( b(:,2) - b(:,1) ); 69 | Ht = eye(3); 70 | Ht(1:2,3) = b(:,1) + 1; 71 | 72 | for k = 1:length(H) 73 | H{k} = H{k}*Ht; 74 | end 75 | 76 | if length(H)==1 77 | H = H{1}; 78 | end 79 | 80 | return 81 | 82 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5 83 | 84 | % D = dethomog(H,q) Determinant of Jacobian of homography transformation r = nhom(H*hom(q)), D = det(dr/dq). Works vector-wise for size(q)=[2 ?]. 85 | % 86 | function D = dethomog(H,q) 87 | % Derivative of homography r = h(q) = nhom(H*[q;1]) is: 88 | % dh = [eye(2) -r]/y(3)*H(:,1:2), where y = H*[q;1]. 89 | q(3,:) = 1; 90 | r = vgg_get_nonhomg(H*q); 91 | D = ((H(1,1)-r(1,:)*H(3,1)).*(H(2,2)-r(2,:)*H(3,2)) - (H(1,2)-r(1,:)*H(3,2)).*(H(2,1)-r(2,:)*H(3,1))) ./ (H(3,:)*q).^2; 92 | return 93 | 94 | 95 | function i = boxclipu(b,u) 96 | % i = boxclipu(b,u) Returns logical indices of points inside box b. 97 | i = all( u >= b(:,row1(u)) & u <= b(:,2*row1(u)), 1 ); 98 | return 99 | 100 | 101 | function ub = bboxu(u) 102 | % ub = bboxu(u) Bounding box of point cloud u. 103 | ub = [min(u')' max(u')']; 104 | return -------------------------------------------------------------------------------- /vgg_multiview/vgg_projective_basis_2d.m: -------------------------------------------------------------------------------- 1 | function H=vgg_projective_basis_2d(p1,p2,p3,p4); 2 | % 3 | % function H=vgg_projective_basis_2d(p1,p2,p3,p4); 4 | % 5 | % Computes 3x3 homography matrix H such that 6 | % 7 | % H * [p1 p2 p3 p4] ~ [ 1 0 0 1 8 | % 0 1 0 1 9 | % 0 0 1 1 ] 10 | % 11 | % fsm@robots 12 | % 13 | 14 | if ( (~ exist('p1')) | (~ exist('p2')) | (~ exist('p3')) | (~ exist('p4')) ) 15 | error('missing input argument(s)'); 16 | end 17 | 18 | if ( prod(size(p1)) == 3 ) 19 | p1=reshape(p1,[3 1]); 20 | else 21 | error('p1 must have 3 entries'); 22 | end 23 | 24 | if ( prod(size(p2)) == 3 ) 25 | p2=reshape(p2,[3 1]); 26 | else 27 | error('p2 must have 3 entries'); 28 | end 29 | 30 | if ( prod(size(p3)) == 3 ) 31 | p3=reshape(p3,[3 1]); 32 | else 33 | error('p3 must have 3 entries'); 34 | end 35 | 36 | if ( prod(size(p4)) == 3 ) 37 | p4=reshape(p4,[3 1]); 38 | else 39 | error('p4 must have 3 entries'); 40 | end 41 | 42 | % 43 | tmp=[p1 p2 p3] \ p4; 44 | p1 = p1*tmp(1); 45 | p2 = p2*tmp(2); 46 | p3 = p3*tmp(3); 47 | 48 | % now p4=p1+p2+p3 49 | 50 | H=inv([p1 p2 p3]); 51 | 52 | return; 53 | -------------------------------------------------------------------------------- /vgg_multiview/vgg_rms_error.m: -------------------------------------------------------------------------------- 1 | function e = vgg_rms_rrror(M) 2 | % e = vgg_rms_rrror(M) 3 | % 4 | % Get RMS diff from zero of matrix or vector M 5 | 6 | e = sqrt(sum(sum(M.*M)) / prod(size(M))); 7 | -------------------------------------------------------------------------------- /vgg_multiview/vgg_scatter_plot.m: -------------------------------------------------------------------------------- 1 | function vgg_scatter_plot(points,style) 2 | %vgg_scatter_plot(points,style) 3 | % 4 | %Plots nonhomg points as a scatter plot 5 | 6 | if nargin == 1, style = '+'; end 7 | [r,c]=size(points); 8 | if r == 2 9 | h = plot(points(1,:),points(2,:),style); 10 | elseif r == 3 11 | h = plot3(points(1,:), points(2,:), points(3,:), style); 12 | else 13 | error('rows != 2 or 3'); 14 | end 15 | -------------------------------------------------------------------------------- /vgg_multiview/vgg_scatter_plot_homg.m: -------------------------------------------------------------------------------- 1 | function vgg_scatter_plot_homg(homg_points, style) 2 | % vgg_scatter_plot_homg(homg_points, style) 3 | % 4 | % Scatter plot homogeneous points 5 | 6 | if nargin == 1, style = '+'; end 7 | 8 | vgg_scatter_plot(vgg_get_nonhomg(homg_points), style); 9 | -------------------------------------------------------------------------------- /vgg_multiview/vgg_selfcalib_metric_vansq.m: -------------------------------------------------------------------------------- 1 | % vgg_selfcalib_metric_vansq Metric selfcalibration from 3 orthogonal principal directions and square pixels. 2 | % 3 | % DESCRIPTION 4 | % Given projective camera matrices P and 3 scene points V, it computes 3D-to-3D 5 | % homography H which upgrades the old reconstruction to metric one, i.e., 6 | % differing from the true one only by isotropic scaling. 7 | % H is computed from the following constraints :- 8 | % (1) points H*V are at infty and mutually orthogonal (i.e., H*V==eye(4,3)), 9 | % (2) cameras P*inv(H) have square pixels, 10 | % Constraint (1) is hard, (2) is soft. I.e., if [P,V] are not consistent 11 | % (2) will be satisfied only partially (in linear least squares sense). 12 | % 13 | % SYNSOPSIS 14 | % [H,sv] = vgg_selfcalib_metric_vansq(P,V), where 15 | % P ... cell{K} of double(3,4), projective cameras 16 | % V ... double(4,3), 3 projective scene points (homog. coords.) 17 | % H ... double(4,4), upgrading 3D-to-3D homography 18 | % sv ... 2-vector, last 2 singular values of linear system solving for square pixels. 19 | % In healthy situation, sv(2) must be tiny and sv(1) reasonably large. 20 | % 21 | % NOTE: To get correct handedness (= non-mirroring) of the reconstruction, 22 | % make sure that V satisfies 23 | % vgg_wedge(V)*[X C] > 0 24 | % for all scene points X and camera centers C(:,k) = vgg_wedge(P{k}). This can be 25 | % achieved by swapping signs of P and X using vgg_signsPX_from_x. The thing 26 | % requires also positive handedness of image and scene coord. systems. 27 | % 28 | % SEE ALSO vgg_selfcalib_qaffine. 29 | 30 | % T.Werner, Feb 2002 31 | 32 | function H = vgg_selfcalib_metric_vansq(P,V) 33 | 34 | K = length(P); 35 | 36 | %%%%%%%%%% 37 | % Step 1: 38 | % Find 3D homography H1 sending V to eye(4,3). 39 | % This results in reconstruction differing from metric one only in scaling in axis directions. 40 | %%%%%%%%%%% 41 | 42 | 43 | H1 = inv(normx([V vgg_wedge(V)'])); 44 | for k = 1:K 45 | P{k} = P{k}/H1; 46 | end 47 | 48 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 49 | % Step 2: 50 | % Find scaling in axis directions from square pixel assumption. 51 | % This scaling is represented by diagonal 3D homography H2. 52 | % 53 | % Algorithm: 54 | % Consider input camera matrix P = [M v] and output matrix Q = K*R*[eye(3) -t]. It is Q =~ P*H where H = diag([d 1]). 55 | % Let O = diag([1 1 1 0]) be absolute quadric. Then Q*O*Q' = DIAC = inv(IAC). For square pixels, IAC(1,1)=IAC(2,2) and IAC(1,2)=0. 56 | % Substitution gives Q*O*Q' =~ P*H*O*H'*P' where P*H*O*H'*P' = M*diag(d.^2)*M' =~ inv(IAC). I.e., 57 | % IAC =~ inv(M)*diag(d.^(-2))*inv(M)'. 58 | % The RHS of the last expression can be rearranged as 59 | % vech(IAC) =~ pinv(duplication(3))*kron(inv(M)',inv(M)')*diagonalize(3)*d, where size(d)=[3 1]. 60 | % This is used to build a system of linear equations. We showed things only for onen camera - more cameras can be added to the system easily. 61 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 62 | 63 | % compose the linear system 64 | A = []; 65 | for k = 1:K 66 | aux = inv(P{k}(:,1:3))'; 67 | aux = pinv(vgg_duplic_matrix(3))*kron(aux,aux)*diagonalize(3); 68 | A = [A; [aux(1,:)-aux(4,:); aux(2,:)]]; 69 | end 70 | 71 | % solve it 72 | [dummy,sv,d] = svd(A,0); 73 | d = d(:,end); 74 | sv = diag(sv); 75 | sv = sv(2:3)/sv(1); % normalize sing values 76 | 77 | % form H 78 | d = 1./sqrt(abs(d)); 79 | H2 = diag([d;1]); 80 | H = inv(H2)*H1; 81 | 82 | return 83 | 84 | %%%%%%%%%%%%%%%%%%%% 85 | 86 | 87 | % G = diagonalize(n) Diagonalization matrix. It is vec(diag(x)) = diagonalize(length(x))*x. 88 | function G = diagonalize(n) 89 | G = zeros(n^2,n); 90 | i = []; 91 | for j = 0:n-1 92 | i = [i 1+n*j+j]; 93 | end 94 | G(i,:) = eye(n); 95 | return 96 | 97 | 98 | % x = normx(x) Normalize MxN matrix so that norm of each its column is 1. 99 | function x = normx(x) 100 | if ~isempty(x) 101 | x = x./(ones(size(x,1),1)*sqrt(sum(x.*x))); 102 | end 103 | return -------------------------------------------------------------------------------- /vgg_multiview/vgg_selfcalib_qaffine.m: -------------------------------------------------------------------------------- 1 | % vgg_selfcalib_qaffine Upgrading projective to quasi-affine reconstruction. 2 | % 3 | % Given projective reconstruction [P,X] with correct signs of P and X 4 | % (the output of vgg_signsPX_from_x), it finds homography H transforming 5 | % [P,X] to quasi-affine reconstruction [Pq,Xq] = [P*inv(H),H*X]. 6 | % Let Ainf=[0 0 0 1] be plane at infinity, then [Pq,Xq] has the property that :- 7 | % 8 | % Ainf * Xq > 0 (all scene points in front of plane at infty) 9 | % Ainf * vgg_wedge(Pq{k}) > 0 (all camera centers in front of plane at infty) 10 | % 11 | % H = vgg_selfcalib_qaffine(P,X), where 12 | % P ... cell(K) of double(3,4), camera matrices. K is number of cameras. 13 | % P also can be 3x4xK array. 14 | % X ... double(4,N), scene points in homog. coordinates. 15 | % H ... cell{I} of double(4,4), homographies upgrading [P,X] to quasi-affine reconstruction. 16 | % There can be 0, 1, or 2 solution classes (corresponding to I=0,1,2) :- 17 | % - I==0 ... no solution, ie [P,X] cannot be transformed to any affine scene. 18 | % - I==1 ... 1 solution, ie camera centers and scene points 19 | % are not separable by a plane in the true scene. 20 | % - I==2 ... 2 solutions, ie camera centers and scene points are separable 21 | % by a plane in the true scene. Then there are two solutions for plane at infinity, 22 | % differing by sign(det(H{i})). The two reconstruction corresponding to H{1} and H{2} 23 | % have oppposite handedness and we cannot say which handedness is that of the true scene. 24 | % (Note: by 'solution' we mean rather 'class of solutions' - indeed there are infinitely many 25 | % solutions if I>0, and linear programming chooses a single solution out of them.) 26 | % 27 | % EXAMPLE: Let [P,X] be a projective reconstruciton from homogeneous image points x. 28 | % Upgrade to quasi-affine reconstruction is done as follows: 29 | % [P,X] = vgg_signsPX_from_x(P,X,x); 30 | % H = vgg_selfcalib_qaffine(P,X); 31 | % H = H{1}; % single solution assumed 32 | % P = P*inv(H); 33 | % X = H*X; 34 | % If either of rows 1 and 2 returns no solution, there's something wrong with 35 | % the reconstruction, eg an outlier. 36 | 37 | % T.Werner, Feb 2002, werner@robots.ox.ac.uk 38 | 39 | function H = vgg_selfcalib_qaffine(P,X) 40 | 41 | if ndims(P)==3 42 | for k = 1:size(P,3) 43 | Q{k} = P(:,:,k); 44 | end 45 | P = Q; 46 | end 47 | 48 | [D N] = size(X); 49 | K = length(P); 50 | 51 | for k = 1:K 52 | C(:,k) = vgg_wedge(P{k}); % oriented camera centers 53 | end 54 | 55 | % Solve chiral equalities: 56 | % 57 | % A := found plane at infinity 58 | % detH := required det(H) 59 | % (A and detH can be none, one, or two according to the number of solution classes) 60 | detH = []; 61 | A = []; 62 | for detHa = [-1 1] 63 | Aa = sephplane([X detHa*C]); 64 | if ~isempty(Aa) 65 | A = [A; Aa]; 66 | detH = [detH; detHa]; 67 | end 68 | end 69 | 70 | if isempty(A) 71 | H = {}; 72 | return 73 | end 74 | 75 | 76 | % compose final homography H 77 | for i = 1:size(A,1) 78 | 79 | % find H{i} such that H{i}(4,:)==A 80 | [dummy,dummy,H{i}] = svd(A(i,:),0); 81 | H{i} = H{i}(end:-1:1,:); 82 | 83 | % make det(H{i}) the same sign as detH(i) 84 | if det(H{i})*detH(i) < 0 85 | H{i} = H{i}([2 1 3 4],:); 86 | end 87 | 88 | % 'beautifier' of X: 89 | % Do singular value equalization on the set X, 90 | % i.e., make mean(nhom(H{i}*X),2)==[0;0;0] and svd(nhom(H{i}*X))==[1;1;1]. 91 | Xi = vgg_get_nonhomg(H{i}*X); 92 | c = mean(Xi,2); % centroid 93 | Xi = Xi - c*ones(1,N); 94 | [U,S] = eig(Xi*Xi'); % sv equalization 95 | S = diag(1./sqrt(diag(S))); 96 | K = S*U'; 97 | if det(K) < 0 % we want the sv equalization to be parity-preserving 98 | K = -K; 99 | end 100 | H{i} = [ K -K*c; 0 0 0 1 ]*H{i}; 101 | 102 | end 103 | 104 | return 105 | 106 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 107 | 108 | 109 | % A = sephplane(X) Finds separating hyperplane A such that all(A*X)>0. 110 | % If no solution exists, A = []. 111 | % Works for any dimension of X. 112 | function A = sephplane(X) 113 | 114 | [D,N] = size(X); 115 | 116 | X = X ./ (ones(D,1)*sqrt(sum(X.*X))); 117 | A = [-X' ones(N,1)]; 118 | b = zeros(size(A,1),1); 119 | f = [zeros(1,D) -1]'; 120 | LB = [-ones(1,D) 0]; 121 | UB = [ones(1,D) Inf]; 122 | fprintf('vgg selfcalib_qaffine: linprog for %d %dd pts ... ', size(X,2), D); 123 | options = optimset('linprog'); 124 | options.Display = 'off'; 125 | [res,FVAL,EXITFLAG] = linprog(f,A,b,[],[],LB,UB, [], options); 126 | if isempty(res) 127 | fprintf('no feasible plane\n'); 128 | A = []; 129 | return 130 | end 131 | A = res(1:D)'; 132 | 133 | if ~all(A*X > 0) 134 | fprintf('feasible plane returned, but is not in fact feasible\n'); 135 | A = []; 136 | end 137 | 138 | fprintf('Got plane [%.2f %.2f %.2f %.2f]\n', A); 139 | 140 | return 141 | 142 | 143 | %i = k2i(k) 144 | % Computes indices of joint point matrix rows corresponding to views k. 145 | %% function i = k2i(k,step) 146 | %% k = k(:)'; 147 | %% i = [1:3]'*ones(size(k)) + 3*(ones(3,1)*k-1); 148 | %% i = i(:); 149 | -------------------------------------------------------------------------------- /vgg_multiview/vgg_signsPX_from_x.m: -------------------------------------------------------------------------------- 1 | % [P,X] = vgg_signsPX_from_x(P,X,x) Finds signs of P and X in a projective reconstruction. 2 | % 3 | % Given a projective reconstruction, i.e. P, X, and x such that 4 | % s_n^k x_n^k = P^k X_n, 5 | % where 6 | % - P^k is k-th camera matrix 7 | % - X_n is n-th scene point 8 | % - x_n^k is image projection of X_n in camera P^k 9 | % - s_n^k is scale, 10 | % it will change signs of P^k and X_n such that all s_n^k are positive. Positivity of s_n^k 11 | % determines the signs of P and X uniquely up to a single overall sign. 12 | % 13 | % Parameters: 14 | % P ... cell(K) of 3-by-4 matrices, camera matrices. 15 | % P also can be (3*K)-by-4 joint camera matrix. 16 | % P also can be 3-by-4-by-K array. 17 | % X ... double(4,N), scene points in homog. coordinates 18 | % x ... cell(K) of double(3,N), image points in homog. coordinates. 19 | % If an image point is missing, set x{k}(:,n) = [NaN;NaN;NaN]. 20 | % x also can be joint (3*K)-by-N joint image point matrix, again with NaNs if a point is missing. 21 | % x also can be 3-by-N-by-K array. 22 | % 23 | % If it is not possible to change signs of P^k and X_n such that s_n^k are positive, it is P=X=[]. 24 | % This means that the projective reconstruction [P,X,x] does not correspond to any real scene. 25 | % 26 | % The function works for any dimension, ie, D-by-(D+1) camera matrices. 27 | % 28 | % See also vgg_selfcalib_qaffine. 29 | 30 | function [P0,X] = vgg_signsPX_from_x(P0,X,u0) 31 | 32 | % Re-arrange input data to joint camera matrix and joint image points. 33 | if iscell(P0) 34 | P = vertcat(P0{:}); 35 | u = vertcat(u0{:}); 36 | else 37 | if ndims(P0)==2 % joint camera matrix 38 | P = P0; 39 | u = u0; 40 | else % P(:,:,k) is k-th camera matrix 41 | P = []; 42 | u = []; 43 | for k = 1:size(P0,3) 44 | P = [P; P0(:,:,k)]; 45 | u = [u; u0(:,:,k)]; 46 | end 47 | end 48 | end 49 | 50 | [D N] = size(X); 51 | K = size(P,1)/(D-1); 52 | 53 | % Do sign swapping in joint image / joint camera matrix format 54 | if any(isnan(u(:))) 55 | [P,X] = signsPX_from_x_occl(P,X,u); % slower code but can handle undefined points 56 | else 57 | [P,X] = signsPX_from_x(P,X,u); % faster code if all image points are defined 58 | end 59 | 60 | if isempty(P) 61 | P0 = []; 62 | return 63 | end 64 | 65 | % Re-arrange back to original format 66 | if iscell(P0) 67 | for k = 1:length(P0) 68 | P0{k} = P([1:D-1]+(D-1)*(k-1),:); 69 | end 70 | else 71 | if ndims(P0)==2 72 | P0 = P; 73 | else 74 | for k = 1:size(P0,3) 75 | P0(:,:,k) = P([1:D-1]+(D-1)*(k-1),:); 76 | end 77 | end 78 | end 79 | 80 | return 81 | 82 | 83 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%% 84 | 85 | 86 | % Does the sign swapping if all image points are defined (ie, no nans are in u). 87 | function [P,X] = signsPX_from_x(P,X,x) 88 | 89 | [D N] = size(X); 90 | K = size(P,1)/(D-1); 91 | 92 | s = sign( reshape( sum(reshape(x,D-1,K*N).*reshape(P*X,D-1,K*N)), K,N) ); 93 | 94 | sP = s(:,1); 95 | s = sP(:,ones(1,N)) .* s; 96 | 97 | sX = s(1,:); 98 | s = sX(ones(1,K),:) .* s; 99 | 100 | if any(s(:)<0) 101 | P = []; 102 | X = []; 103 | return 104 | end 105 | 106 | aux = sP(:,ones(1,D-1))'; aux = aux(:); P = P .* aux(:,ones(1,D)); 107 | X = X .* sX(ones(1,D),:); 108 | 109 | return 110 | 111 | 112 | % Does sign swapping if there are undefined points (nans) in x. 113 | function [P,X] = signsPX_from_x_occl(P,X,u) 114 | 115 | [D N] = size(X); 116 | K = size(P,1)/(D-1); 117 | 118 | PX = reshape( dot(reshape(u,D-1,K*N),reshape(P*X,D-1,K*N)), K,N); 119 | for initp = 1:K 120 | p = NaN*ones(K,1); 121 | x = NaN*ones(1,N); 122 | p(initp) = 1; 123 | n = 1; 124 | oldn = 0; 125 | while n-oldn > 0 126 | oldn = n; 127 | x = updatej(p,x,PX); 128 | p = updatej(x',p',PX')'; 129 | n = nnz(~isnan([p' x])); 130 | end 131 | if all(all(isnan(PX) | ~isnan((p*x).*PX))) 132 | break 133 | end 134 | end 135 | P = (reshape(ones(D-1,1)*p',(D-1)*K,1)*ones(1,D)) .* P; 136 | X = (ones(D,1)*x) .* X; 137 | 138 | % check if the sign changing process was succesful 139 | uPX = reshape( dot(reshape(u,D-1,K*N),reshape(P*X,D-1,K*N)), K,N); 140 | if ~all(all( (uPX>0) | isnan(uPX) )) 141 | P = []; 142 | X = []; 143 | end 144 | 145 | return 146 | 147 | 148 | % Auxiliary function used for swaping the signs of P^k, X_n. 149 | function j = updatej(i,j,IJ) 150 | mj = any(~isnan(i*ones(1,length(j))) & isnan(ones(length(i),1)*j) & ~isnan(IJ)); 151 | nj = IJ(:,mj) .* (i*ones(1,nnz(mj))); 152 | nj_ = nj(:); nj_(isnan(nj_)) = 0; nj(:) = nj_; 153 | j(mj) = sign(sum(nj)); 154 | return 155 | -------------------------------------------------------------------------------- /vgg_numerics/Contents.m: -------------------------------------------------------------------------------- 1 | % VGG numerics library 2 | % 3 | % Linear equation system solving 4 | % vgg_mrdivs - solving for matrix A from equation system y =~ A*x (equality up to scale) 5 | % 6 | % Rearranging matrix equations 7 | % vgg_vec - vectorize matrix, same as (:) operator, and its inverse for square matrix 8 | % vgg_vech - as vec, takes only entries on and under diagonal 9 | % vgg_commut_matrix - it is vec(X') = commut_matrix(size(X))*vec(X) 10 | % vgg_duplic_matrix - for square X, it is duplic_matrix(n)*vech(X) = vec(X) 11 | % vgg_vech_swap - for square A, it is x'*A*y = vec(A)'*vec_swap(x,y), vectorized 12 | % vgg_vec_swap - for square symmetric A, it is x'*A*y = vech(A)'*vech_swap(x,y), vectorized 13 | % 14 | % Misc 15 | % vgg_gauss_mask - univariate gaussian or any its derivative 16 | % vgg_diagonalize_conic - transforms conic to canonical position 17 | % vgg_wedge - wedge (Grassmann outer) product of N-1 N-vectors 18 | -------------------------------------------------------------------------------- /vgg_numerics/vgg_commut_matrix.m: -------------------------------------------------------------------------------- 1 | % VGG_COMMUT_MATRIX Commutation matrix, to transpose a matrix(:) 2 | % 3 | % Classical matrix re-arrangement operator, see book Magnus-Neudecker. 4 | % 5 | % Useful for rearranging matrix equations. It is 6 | % vgg_vec(X') = vgg_commut_matrix(size(X))*vgg_vec(X). 7 | % 8 | % See also vgg_matrix_test 9 | 10 | % Added by Tom Werner, originally from Tomas Minka. 11 | 12 | function k = vgg_commut_matrix(n, m) 13 | 14 | if nargin < 2 15 | m = n(2); 16 | n = n(1); 17 | end 18 | 19 | k = reshape(kron(vgg_vec(eye(n)), eye(m)), n*m, n*m); 20 | 21 | return 22 | -------------------------------------------------------------------------------- /vgg_numerics/vgg_contreps.m: -------------------------------------------------------------------------------- 1 | function Y = vgg_contreps(X) 2 | 3 | % vgg_contreps Contraction with epsilon tensor. 4 | % 5 | % B = vgg_contreps(A) is tensor obtained by contraction of A with epsilon tensor. 6 | % However, it works only if the argument and result fit to matrices, in particular: 7 | % 8 | % - if A is row or column 3-vector ... B = [A]_x 9 | % - if A is skew-symmetric 3-by-3 matrix ... B is row 3-vector such that A = [B]_x 10 | % - if A is skew-symmetric 4-by-4 matrix ... then A can be interpreted as a 3D line Pluecker matrix 11 | % skew-symmetric 4-by-4 B as its dual Pluecker matrix. 12 | % - if A is row 2-vector ... B = [0 1; -1 0]*A', i.e., A*B=eye(2) 13 | % - if A is column 2-vector ... B = A'*[0 1; -1 0], i.e., B*A=eye(2) 14 | % 15 | % It is vgg_contreps(vgg_contreps(A)) = A. 16 | 17 | % werner@robots.ox.ac.uk, Oct 2001 18 | 19 | if prod(size(X)) == 3 % get [X]_\times 20 | Y = [0 X(3) -X(2) 21 | -X(3) 0 X(1) 22 | X(2) -X(1) 0]; 23 | elseif all(size(X) == [1 2]) 24 | Y = [0 1; -1 0]*X'; 25 | elseif all(size(X) == [2 1]) 26 | Y = X'*[0 1; -1 0]; 27 | elseif all(size(X) == [3 3]) % get X from [X]_\times 28 | Y = [X(2,3) X(3,1) X(1,2)]; 29 | elseif all(size(X) == [4 4]) % pluecker matrix dual 30 | Y = [0 X(3,4) X(4,2) X(2,3) 31 | X(4,3) 0 X(1,4) X(3,1) 32 | X(2,4) X(4,1) 0 X(1,2) 33 | X(3,2) X(1,3) X(2,1) 0 ]; 34 | else 35 | error('Wrong matrix size.') 36 | end -------------------------------------------------------------------------------- /vgg_numerics/vgg_detn.cxx: -------------------------------------------------------------------------------- 1 | #include "mex.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | //void ludcmp(double **a, int n, int *indx, float *d) 8 | void ludcmp(double **a, int n, double *d, double *vv) 9 | { 10 | int i,imax,j,k; 11 | double big,dum,sum,temp; 12 | 13 | *d=1.0; 14 | for (i=1;i<=n;i++) { 15 | big=0.0; 16 | for (j=1;j<=n;j++) 17 | if ((temp=fabs(a[i][j])) > big) big=temp; 18 | if (big == 0.0) mexErrMsgTxt("Singular matrix in routine ludcmp"); 19 | vv[i]=1.0/big; 20 | } 21 | for (j=1;j<=n;j++) { 22 | for (i=1;i= big) { 34 | big=dum; 35 | imax=i; 36 | } 37 | } 38 | if (j != imax) { 39 | for (k=1;k<=n;k++) { 40 | dum=a[imax][k]; 41 | a[imax][k]=a[j][k]; 42 | a[j][k]=dum; 43 | } 44 | *d = -(*d); 45 | vv[imax]=vv[j]; 46 | } 47 | //indx[j]=imax; 48 | if (a[j][j] == 0.0) a[j][j]=1.0e-20; 49 | if (j != n) { 50 | dum=1.0/(a[j][j]); 51 | for (i=j+1;i<=n;i++) a[i][j] *= dum; 52 | } 53 | } 54 | 55 | } 56 | 57 | 58 | /* D = detn(X) vectorized determinant; D(i1,...,in) = det(X(:,:,i1,...,in)). */ 59 | void mexFunction( int nargout, 60 | mxArray **argout, 61 | int nargin, 62 | const mxArray **argin 63 | ) 64 | { 65 | int ndims, n, i, j, k, N; 66 | const int *dim; 67 | double *X, *x, *D, **a, *vv, *d; 68 | 69 | if ( nargin != 1 ) mexErrMsgTxt("One input parameter required."); 70 | 71 | if ( mxGetClassID(argin[0]) != mxDOUBLE_CLASS ) mexErrMsgTxt("X must be double."); 72 | ndims = mxGetNumberOfDimensions(argin[0]); 73 | if ( ndims < 2 ) mexErrMsgTxt("X must have 2+ dimensions."); 74 | dim = mxGetDimensions(argin[0]); 75 | if ( dim[0]!=dim[1] ) mexErrMsgTxt("X must have first two dimensions equal."); 76 | X = (double*)mxGetPr(argin[0]); 77 | 78 | if ( ndims>2 ) 79 | argout[0] = mxCreateNumericArray(ndims-2,dim+2,mxDOUBLE_CLASS,mxREAL); 80 | else 81 | argout[0] = mxCreateDoubleMatrix(1,1,mxREAL); 82 | if ( argout[0]==0 ) mexErrMsgTxt("Out of memory."); 83 | D = (double*)mxGetPr(argout[0]); 84 | 85 | // Allocate aux. array a 86 | a = (double**)malloc((dim[0]+1)*sizeof(double*)); 87 | for ( i = 0; i <= dim[0]; i++ ) a[i] = (double*)malloc((dim[1]+1)*sizeof(double)); 88 | 89 | vv=(double*)malloc((dim[0]+1)*sizeof(double)); 90 | 91 | N = 1; for ( i = 2; i < ndims; i++ ) N *= dim[i]; // N = number of determinants 92 | for ( n = 0, x = X, d = D; n < N; n++, x+=dim[0]*dim[1], d++ ) { 93 | if ( dim[0]==2 ) 94 | *d = x[0]*x[3]-x[2]*x[1]; 95 | else if ( dim[0]==3 ) 96 | *d = x[0]*x[4]*x[8]+x[2]*x[3]*x[7]+x[1]*x[5]*x[6]-x[2]*x[4]*x[6]-x[1]*x[3]*x[8]-x[0]*x[5]*x[7]; 97 | else { 98 | k = 0; for ( i = 1; i <= dim[0]; i++) for ( j = 1; j <= dim[1]; j++ ) a[j][i] = x[k++]; 99 | ludcmp(a,dim[0],d,vv); 100 | for ( i=1;i<=dim[0];i++) *d *= a[i][i]; 101 | } 102 | } 103 | 104 | free(vv); 105 | for ( i = 0; i < dim[0]; i++ ) free(a[i]); 106 | free(a); 107 | } 108 | -------------------------------------------------------------------------------- /vgg_numerics/vgg_detn.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jmmanley/VGG-Multiple-View-Geometry/f114712de03082bb97229eaf2a65981908b64127/vgg_numerics/vgg_detn.dll -------------------------------------------------------------------------------- /vgg_numerics/vgg_diagonalize_conic.m: -------------------------------------------------------------------------------- 1 | % H = vgg_diagonalize_conic(C) Finds Euclidean transformation sending conic canonical position. 2 | % 3 | % For any symmetric matrix C, returns Euclidean transformation H such that 4 | % H'*C*H 5 | % is a diagonal matrix. 6 | % 7 | % Typical usage is to transform conics to canonical form, to classify or plot them. 8 | 9 | function H = vgg_diagonalize_conic(C) 10 | 11 | N = size(C,1); 12 | 13 | C = C/C(end,end); 14 | s = C(1:end-1,end); 15 | Q = C(1:end-1,1:end-1); 16 | 17 | [U,S,V] = svd(Q); 18 | sw = diag([-1 ones(1,N-2)]); 19 | if det(U) < 0 20 | U = U*sw; 21 | S = sw*S; 22 | end 23 | if det(V) < 0 24 | V = V*sw; 25 | S = S*sw; 26 | end 27 | 28 | H = [U -inv(Q)*s; zeros(1,N-1) 1]; 29 | 30 | return -------------------------------------------------------------------------------- /vgg_numerics/vgg_duplic_matrix.m: -------------------------------------------------------------------------------- 1 | % d = vgg_duplic_matrix(n) Duplication matrix. 2 | % 3 | % Classical matrix re-arrangement operator, see book Magnus-Neudecker. 4 | % 5 | % Useful for rearranging equations with symmetric matrices. 6 | % For square symmetric X, it is 7 | % 8 | % vgg_duplic_matrix(n)*vgg_vech(X) = vgg_vec(X) 9 | % 10 | % See also vgg_matrix_test, vgg_vech_swap. 11 | 12 | % Added by Tom Werner, originaly from T.Minka. 13 | 14 | 15 | function d = vgg_duplic_matrix(n) 16 | 17 | a = tril(ones(n)); 18 | i = find(a); 19 | a(i) = 1:length(i); 20 | a = a + tril(a,-1)'; 21 | j = a(:); 22 | 23 | m = n*(n+1)/2; 24 | d = zeros(n*n,m); 25 | for r = 1:size(d,1) 26 | d(r, j(r)) = 1; 27 | end 28 | -------------------------------------------------------------------------------- /vgg_numerics/vgg_gauss_mask.m: -------------------------------------------------------------------------------- 1 | function Y = vgg_gauss_mask(a,der,X,flag) 2 | 3 | %VGG_GAUSS_MASK Univariate Gaussian function or its derivatives. 4 | % Y = vgg_gausmask(sigma,der,X) yields der-th derivative of Gaussian. 5 | % For der==0, plain Gaussian is yielded. 6 | % If X is omitted, X=[-3*sigma:3*sigma] is used, giving reasonably small tail. 7 | % If der is omitted too, der=0 is used. 8 | % 9 | % Try plot(vgg_gauss_mask(10,6,-100:100)) 10 | 11 | % Tom Werner, Oct 2001 12 | 13 | 14 | %% 15 | % For computing derivatives, the following recurrent formulas are used: 16 | % gausmask(sigma,0,X) = exp(a*X.^2)/(sigma*sqrt(2*pi)), 17 | % gausmask(sigma,n,X) = 2*a*( (der-1)*gausmask(sigma,der-2,X) + X.*gausmask(sigma,der-1,X) ), 18 | % where 19 | % a = -0.5/(a*a). 20 | %% 21 | 22 | if nargin == 4 23 | if der > 0 24 | Y = 2*a*( (der-1)*vgg_gauss_mask(a,der-2,X,0) + X.*vgg_gauss_mask(a,der-1,X,0) ); 25 | elseif der == 0 26 | Y = ones(size(X)); 27 | else 28 | Y = zeros(size(X)); 29 | end 30 | else 31 | if nargin < 3 32 | X = ceil(3*a); 33 | X = -X:X; 34 | end 35 | if nargin < 2 36 | der = 0; 37 | end 38 | aa = -0.5/(a*a); 39 | Y = exp(aa*X.^2)/(a*sqrt(2*pi)) .* vgg_gauss_mask(aa,der,X,0); 40 | end 41 | 42 | return -------------------------------------------------------------------------------- /vgg_numerics/vgg_intersect_quadrics.m: -------------------------------------------------------------------------------- 1 | function X = vgg_intersect_quadrics(A, B, C); 2 | % 3 | % function X = vgg_intersect_quadrics(A, B, C); 4 | % 5 | % Purpose: 6 | % Compute intersections of three quadrics in projective 3-space. 7 | % 8 | % Input: 9 | % A, B, C are symmetric 4x4 matrices. 10 | % 11 | % Output: 12 | % X is 4-by-8 complex. Each column is a root. 13 | % 14 | % The function works by converting the problem into an 8x8 generalized 15 | % eigen-problem. 16 | % 17 | % fsm@robots.ox.ac.uk 18 | % 19 | 20 | if any(size(A) ~= [4 4]) 21 | error('bad A') 22 | end 23 | if any(size(B) ~= [4 4]) 24 | error('bad B') 25 | end 26 | if any(size(C) ~= [4 4]) 27 | error('bad C') 28 | end 29 | 30 | % symmetrize and normalize. 31 | A = A + A.'; A = A / norm(A, 'fro'); a = [A(1, 1) 2*A(1, 2) 2*A(1, 3) 2*A(1, 4) A(2, 2) 2*A(2, 3) 2*A(2, 4) A(3, 3) 2*A(3, 4) A(4, 4)].'; 32 | B = B + B.'; B = B / norm(B, 'fro'); b = [B(1, 1) 2*B(1, 2) 2*B(1, 3) 2*B(1, 4) B(2, 2) 2*B(2, 3) 2*B(2, 4) B(3, 3) 2*B(3, 4) B(4, 4)].'; 33 | C = C + C.'; C = C / norm(C, 'fro'); c = [C(1, 1) 2*C(1, 2) 2*C(1, 3) 2*C(1, 4) C(2, 2) 2*C(2, 3) 2*C(2, 4) C(3, 3) 2*C(3, 4) C(4, 4)].'; 34 | 35 | % make multiplication tables (for degrees 1+2=3, 2+2=4 and 3+1=4) 36 | if 1 37 | ijk = [ 38 | 1 39 | 22 40 | 43 41 | 64 42 | 82 43 | 105 44 | 126 45 | 147 46 | 163 47 | 186 48 | 208 49 | 229 50 | 244 51 | 267 52 | 289 53 | 310 54 | 325 55 | 351 56 | 372 57 | 393 58 | 406 59 | 432 60 | 454 61 | 475 62 | 487 63 | 513 64 | 535 65 | 556 66 | 568 67 | 594 68 | 617 69 | 638 70 | 649 71 | 675 72 | 698 73 | 719 74 | 730 75 | 756 76 | 779 77 | 800 78 | ]; 79 | X12 = zeros(20, 4, 10); X12(ijk) = 1; 80 | 81 | ijk = [ 82 | 1 83 | 37 84 | 73 85 | 109 86 | 145 87 | 181 88 | 217 89 | 253 90 | 289 91 | 325 92 | 352 93 | 390 94 | 426 95 | 462 96 | 501 97 | 537 98 | 573 99 | 609 100 | 645 101 | 681 102 | 703 103 | 741 104 | 778 105 | 814 106 | 852 107 | 889 108 | 925 109 | 962 110 | 998 111 | 1034 112 | 1054 113 | 1092 114 | 1129 115 | 1165 116 | 1203 117 | 1240 118 | 1276 119 | 1313 120 | 1349 121 | 1385 122 | 1405 123 | 1446 124 | 1482 125 | 1518 126 | 1561 127 | 1597 128 | 1633 129 | 1669 130 | 1705 131 | 1741 132 | 1756 133 | 1797 134 | 1834 135 | 1870 136 | 1912 137 | 1949 138 | 1985 139 | 2022 140 | 2058 141 | 2094 142 | 2107 143 | 2148 144 | 2185 145 | 2221 146 | 2263 147 | 2300 148 | 2336 149 | 2373 150 | 2409 151 | 2445 152 | 2458 153 | 2499 154 | 2537 155 | 2573 156 | 2614 157 | 2652 158 | 2688 159 | 2726 160 | 2762 161 | 2798 162 | 2809 163 | 2850 164 | 2888 165 | 2924 166 | 2965 167 | 3003 168 | 3039 169 | 3077 170 | 3113 171 | 3149 172 | 3160 173 | 3201 174 | 3239 175 | 3275 176 | 3316 177 | 3354 178 | 3390 179 | 3428 180 | 3464 181 | 3500 182 | ]; 183 | X22 = zeros(35, 10, 10); X22(ijk) = 1; 184 | 185 | ijk = [ 186 | 1 187 | 37 188 | 73 189 | 109 190 | 145 191 | 181 192 | 217 193 | 253 194 | 289 195 | 325 196 | 361 197 | 397 198 | 433 199 | 469 200 | 505 201 | 541 202 | 577 203 | 613 204 | 649 205 | 685 206 | 702 207 | 740 208 | 776 209 | 812 210 | 851 211 | 887 212 | 923 213 | 959 214 | 995 215 | 1031 216 | 1071 217 | 1107 218 | 1143 219 | 1179 220 | 1215 221 | 1251 222 | 1287 223 | 1323 224 | 1359 225 | 1395 226 | 1403 227 | 1441 228 | 1478 229 | 1514 230 | 1552 231 | 1589 232 | 1625 233 | 1662 234 | 1698 235 | 1734 236 | 1772 237 | 1809 238 | 1845 239 | 1882 240 | 1918 241 | 1954 242 | 1991 243 | 2027 244 | 2063 245 | 2099 246 | 2104 247 | 2142 248 | 2179 249 | 2215 250 | 2253 251 | 2290 252 | 2326 253 | 2363 254 | 2399 255 | 2435 256 | 2473 257 | 2510 258 | 2546 259 | 2583 260 | 2619 261 | 2655 262 | 2692 263 | 2728 264 | 2764 265 | 2800 266 | ]; 267 | X31 = zeros(35, 20, 4); X31(ijk) = 1; 268 | 269 | clear ijk 270 | end 271 | 272 | % compose ideal in degree 2. 273 | I2 = [a b c]; 274 | 275 | % compute ideal in degrees 3 and 4. 276 | I3 = reshape(reshape(X12, [20* 4 10]) * I2, [20 3* 4]); 277 | I4 = reshape(reshape(X22, [35*10 10]) * I2, [35 3*10]); 278 | 279 | % the columns of C3 and C4 span complements of I3 and I4, respectively. 280 | [U,S,V] = svd(I3.'); C3 = V(:, 13:20); %W3 = diag(S).' 281 | [U,S,V] = svd(I4.'); C4 = V(:, 28:35); %W4 = diag(S).' 282 | 283 | % make the four multiplication operators. 284 | M = cell(1, 4); 285 | for k=1:4 286 | M{k} = C4.' * X31(:, :, k) * C3; 287 | end 288 | 289 | % FIXME: here we only use M{1} and M{2} to compute the Schur 290 | % decomposition although we use all four multiplication operators to 291 | % extract the root coordinates. 292 | [AA, BB, Q, Z] = qz(M{1}, M{2}); 293 | for k=1:4 294 | M{k} = Q*M{k}*Z; 295 | end 296 | %M{:} 297 | 298 | X = zeros(4, 8); 299 | X(1, :) = diag(M{1}).'; 300 | X(2, :) = diag(M{2}).'; 301 | X(3, :) = diag(M{3}).'; 302 | X(4, :) = diag(M{4}).'; 303 | 304 | % normalize 305 | X = X ./ repmat(max(abs(X), [], 1), [4 1]); 306 | 307 | return; 308 | 309 | -------------------------------------------------------------------------------- /vgg_numerics/vgg_lmultiply_matrix.m: -------------------------------------------------------------------------------- 1 | function M = vgg_lmultiply_matrix(A, size_B) 2 | % VGG_LMULTIPLY_MATRIX Return M such that M*vec(B) = vec(A*B) 3 | % M = VGG_LMULTIPLY_MATRIX(A, size(B)) 4 | % 5 | % See also vgg_matrix_test, vgg_rmultiply_matrix 6 | 7 | % Author: awf@robots.ox.ac.uk 8 | 9 | if isfinite(size_B(1)) 10 | if size_B(1) ~= size(A,2) 11 | error 12 | end 13 | end 14 | 15 | if ~issparse(A) 16 | E = eye(size_B(2)); 17 | else 18 | E = speye(size_B(2)); 19 | end 20 | 21 | M = kron(E, A); 22 | -------------------------------------------------------------------------------- /vgg_numerics/vgg_matrix_test.m: -------------------------------------------------------------------------------- 1 | function vgg_matrix_test 2 | % Test the matrix "flattening" functions from Magnus and Neudecker 3 | % vgg_vec 4 | % vgg_vech 5 | % vgg_commut_matrix 6 | % vgg_duplic_matrix 7 | % vgg_lmultiply_matrix 8 | 9 | % Author: awf@robots.ox.ac.uk 10 | 11 | A = randn(4,5); 12 | B = randn(5,2); 13 | C = randn(2,4); 14 | 15 | fprintf('vgg_matrix_test: BEGIN\n'); 16 | assert('vgg_vec(A*B*C)', 'kron(C'',A)*vgg_vec(B)'); 17 | assert('vgg_vec(A*B)', 'vgg_lmultiply_matrix(A,size(B))*vgg_vec(B)'); 18 | assert('vgg_vec(B*C)', 'vgg_rmultiply_matrix(C,size(B))*vgg_vec(B)'); 19 | 20 | 21 | % Now try to solve A X + X B = C: 22 | X_true = randn(4,4); 23 | A = randn(4,4); 24 | B = randn(4,4); 25 | C = A*X_true + X_true*B; 26 | 27 | % Re-express in terms of x = vec(X): 28 | % fA x + gB x = vec(C) 29 | % so 30 | % x = (fA + gB) \ vec(C) 31 | size_X = [4 4]; % need to know size(X) to convert the matrix multiplys 32 | fA = vgg_lmultiply_matrix(A, size_X); 33 | gB = vgg_rmultiply_matrix(B, size_X); 34 | x = (fA + gB)\vgg_vec(C); 35 | 36 | % Now check we got it right... 37 | assert('x', 'vgg_vec(X_true)') 38 | 39 | 40 | 41 | fprintf('vgg_matrix_test: END\n'); 42 | 43 | function assert(sx,sy) 44 | x = evalin('caller', sx); 45 | y = evalin('caller', sy); 46 | err = norm(x-y); 47 | if err > 1e-12 48 | dbstack 49 | fprintf('vgg_matrix_test: FAILED %s = %s\n', sx, sy); 50 | keyboard 51 | else 52 | fprintf('vgg_matrix_test: PASSED %s = %s\n', sx, sy); 53 | end 54 | -------------------------------------------------------------------------------- /vgg_numerics/vgg_mrdivs.m: -------------------------------------------------------------------------------- 1 | % vgg_mrdivs Solves equation system Y*diag(s) = A*X with unkowns A, s. 2 | % 3 | % A = vgg_mrdivs(X,Y) solves (overdetermined) equation system Y*diag(s) = A*X 4 | % by linear method (DLT algorithm). 5 | % Parameters: 6 | % X ... double (N,K) 7 | % Y ... double (M,K) 8 | % A ... double (M,N) 9 | % s ... double (1,K) 10 | % 11 | % Preconditioning of the points not included in the function. Use vgg_conditioner_*. 12 | % 13 | % Typical usage: 14 | % 1. Estimating an image homography from K pairs of corresponding points. 15 | % If 3-by-K matrices x and y are the points in homogeneous coordinates, the 3-by-3 homography 16 | % matrix is obtained as H = vgg_mrdivs(x,y). 17 | % 18 | % 2. Estimating 3-by-4 camera projection matrix P from corresponding pairs of image and scene points. 19 | % For image points x (3xK matrix) and scene points X (4xK matrix) do P = vgg_mrdivs(X,x). 20 | 21 | % (c) werner@robots.ox.ac.uk 22 | 23 | % Algorithm: 24 | % 25 | % For n-th point pair X(:,n) and Y(:,n) we have 26 | % s*X(:,n) = A*Y(:,n) 27 | % We eliminate s what results in MY*(MY-1)/2 linear homogenous equations 28 | % for elements of A. We solve this system by svd or eig. 29 | 30 | function A = vgg_mrdivs(X,Y) 31 | 32 | [MX,N] = size(X); 33 | [MY,NY] = size(Y); 34 | if N ~= NY, error('Matrices A, B must have equal number of columns.'); end 35 | 36 | % construct the measurement matrix 37 | W = zeros(MX*MY,MY*(MY-1)/2*N); 38 | k = 1; 39 | for i = 1:MY 40 | for j = 1:i-1 41 | W([[1:MX]+MX*(j-1) [1:MX]+MX*(i-1)],[1:N]+N*(k-1)) = [(ones(MX,1)*Y(i,:)).*X; -(ones(MX,1)*Y(j,:)).*X]; 42 | k = k+1; 43 | end 44 | end 45 | 46 | % solve the system || A'(:)' * W || ---> min 47 | [dummy,s,A] = svd(W',0); 48 | A = reshape(A(:,end),MX,MY)'; 49 | 50 | return -------------------------------------------------------------------------------- /vgg_numerics/vgg_quat_from_rotation_matrix.m: -------------------------------------------------------------------------------- 1 | function q = vgg_quat_from_rotation_matrix( R ) 2 | % vgg_quat_from_rotation_matrix Generates quaternion from rotation matrix 3 | % q = vgg_quat_from_rotation_matrix(R) 4 | 5 | q = [ (1 + R(1,1) + R(2,2) + R(3,3)) 6 | (1 + R(1,1) - R(2,2) - R(3,3)) 7 | (1 - R(1,1) + R(2,2) - R(3,3)) 8 | (1 - R(1,1) - R(2,2) + R(3,3)) ]; 9 | 10 | if ~issym(q) 11 | % Pivot to avoid division by small numbers 12 | [b I] = max(abs(q)); 13 | else 14 | % For symbolic quats, just make sure we're nonzero 15 | for k=1:4 16 | if q(k) ~= 0 17 | I = k; 18 | break 19 | end 20 | end 21 | end 22 | 23 | q(I) = sqrt(q(I)) / 2 ; 24 | 25 | if I == 1 26 | q(2) = (R(3,2) - R(2,3)) / (4*q(I)); 27 | q(3) = (R(1,3) - R(3,1)) / (4*q(I)); 28 | q(4) = (R(2,1) - R(1,2)) / (4*q(I)); 29 | elseif I==2 30 | q(1) = (R(3,2) - R(2,3)) / (4*q(I)); 31 | q(3) = (R(2,1) + R(1,2)) / (4*q(I)); 32 | q(4) = (R(1,3) + R(3,1)) / (4*q(I)); 33 | elseif I==3 34 | q(1) = (R(1,3) - R(3,1)) / (4*q(I)); 35 | q(2) = (R(2,1) + R(1,2)) / (4*q(I)); 36 | q(4) = (R(3,2) + R(2,3)) / (4*q(I)); 37 | elseif I==4 38 | q(1) = (R(2,1) - R(1,2)) / (4*q(I)); 39 | q(2) = (R(1,3) + R(3,1)) / (4*q(I)); 40 | q(3) = (R(3,2) + R(2,3)) / (4*q(I)); 41 | end 42 | -------------------------------------------------------------------------------- /vgg_numerics/vgg_quat_matrix.m: -------------------------------------------------------------------------------- 1 | function PM = vgg_quat_matrix(p) 2 | % VGG_QUAT_MATRIX Quaternion multiplication matrix 3 | % PM = vgg_quat_matrix(q) returns PM such that 4 | % PM * q = vgg_quat_mul(p,q) 5 | 6 | % awf@robots.ox.ac.uk, 30/01/05 7 | 8 | PM = [ 9 | [ p(1), -p(2), -p(3), -p(4)] 10 | [ p(2), p(1), -p(4), p(3)] 11 | [ p(3), p(4), p(1), -p(2)] 12 | [ p(4), -p(3), p(2), p(1)] 13 | ]; 14 | -------------------------------------------------------------------------------- /vgg_numerics/vgg_quat_mul.m: -------------------------------------------------------------------------------- 1 | function qout = vgg_quat_mul(r,q) 2 | % VGG_QUAT_MUL Quaternion multiplication 3 | % pq = vgg_quat_mul(p,q) 4 | 5 | % awf@robots.ox.ac.uk, 30/01/05 6 | 7 | qout = ... 8 | [(r(1)*q(1) - r(2)*q(2) - r(3)*q(3) - r(4)*q(4)) 9 | (r(1)*q(2) + r(2)*q(1) + r(3)*q(4) - r(4)*q(3)) 10 | (q(3)*r(1) - q(4)*r(2) + q(1)*r(3) + q(2)*r(4)) 11 | (q(4)*r(1) + q(3)*r(2) - q(2)*r(3) + q(1)*r(4)) ]; 12 | -------------------------------------------------------------------------------- /vgg_numerics/vgg_quat_rotation_matrix.m: -------------------------------------------------------------------------------- 1 | function R = vgg_quat_rotation_matrix(q) 2 | % vgg_quat_rotation_matrix Generates rotation matrix from quaternion 3 | % R = vgg_quat_rotation_matrix(q) 4 | 5 | q00 = q(1)*q(1); 6 | q0x = q(1)*q(2); 7 | q0y = q(1)*q(3); 8 | q0z = q(1)*q(4); 9 | qxx = q(2)*q(2); 10 | qxy = q(2)*q(3); 11 | qxz = q(2)*q(4); 12 | qyy = q(3)*q(3); 13 | qyz = q(3)*q(4); 14 | qzz = q(4)*q(4); 15 | 16 | R = [ q00 + qxx - qyy - qzz 2*(qxy - q0z) 2*(qxz + q0y) 17 | 2*(qxy + q0z) q00 - qxx + qyy - qzz 2*(qyz - q0x) 18 | 2*(qxz - q0y) 2*(qyz + q0x) q00 - qxx - qyy + qzz ]; 19 | -------------------------------------------------------------------------------- /vgg_numerics/vgg_quaternion_demo.m: -------------------------------------------------------------------------------- 1 | 2 | 3 | % Make some synthetic quaternions 4 | syms q0 q1 q2 q3 p0 p1 p2 p3 real 5 | 6 | q = [q0 q1 q2 q3]'; 7 | p = [p0 p1 p2 p3]'; 8 | 9 | vgg_assert_equal('vgg_quat_mul(p,q)', 'vgg_quat_matrix(p) * q', 1e-8,1); 10 | 11 | % Generate a random unit quaternion. 12 | % This represents a 3D rotation. 13 | q = randn(4,1); 14 | q = q / norm(q); 15 | 16 | % Now convert it to a rotation matrix, 17 | % and convert back to ensure these routines 18 | % are self-inverse 19 | R = vgg_quat_rotation_matrix(q); 20 | qback = vgg_quat_from_rotation_matrix(R); 21 | 22 | vgg_assert_equal(q, qback, -1e-8,1); 23 | 24 | -------------------------------------------------------------------------------- /vgg_numerics/vgg_rmultiply_matrix.m: -------------------------------------------------------------------------------- 1 | function M = vgg_lmultiply_matrix(A, size_B) 2 | % VGG_LMULTIPLY_MATRIX Return M such that M*vec(B) = vec(B*A) 3 | % M = VGG_RMULTIPLY_MATRIX(A, size(B)) 4 | % 5 | % See also vgg_matrix_test, vgg_lmultiply_matrix 6 | 7 | % Author: awf@robots.ox.ac.uk 8 | 9 | if isfinite(size_B(1)) 10 | if size_B(2) ~= size(A,1) 11 | error 12 | end 13 | end 14 | 15 | if ~issparse(A) 16 | E = eye(size_B(1)); 17 | else 18 | E = speye(size_B(1)); 19 | end 20 | 21 | M = kron(A', E); 22 | -------------------------------------------------------------------------------- /vgg_numerics/vgg_rotmat_from_exp.m: -------------------------------------------------------------------------------- 1 | function M = vgg_rotmat_from_exp(r) 2 | % VGG_ROTMAT_FROM_EXP Convert from exponential to matrix rotation parameterization. 3 | % R = vgg_rotmat_from_exp([r1, r2, r3]) generates the rotation 4 | % matrix with axis along r, angle = norm(r). 5 | % This is equivalent to expm(cross_matrix(r)) but more stable. 6 | 7 | % Andrew Fitzgibbon 8 | 9 | H = [0, -r(3), r(2); r(3), 0, -r(1); -r(2), r(1), 0]; 10 | 11 | if 1 12 | angle = norm(r); 13 | if (angle < eps) 14 | M=eye(3,3); 15 | else 16 | ef = sin(angle)/angle; 17 | gee = (1.0 - cos(angle))/ (angle*angle); 18 | M = (H*H)*gee + H*ef + eye(3,3); 19 | end 20 | else 21 | M = expm(H); 22 | end 23 | -------------------------------------------------------------------------------- /vgg_numerics/vgg_rq.m: -------------------------------------------------------------------------------- 1 | % [R,Q] = vgg_rq(S) Just like qr but the other way around. 2 | % 3 | % If [R,Q] = vgg_rq(X), then R is upper-triangular, Q is orthogonal, and X==R*Q. 4 | % Moreover, if S is a real matrix, then det(Q)>0. 5 | 6 | % By awf 7 | 8 | function [U,Q] = rq(S) 9 | 10 | S = S'; 11 | [Q,U] = qr(S(end:-1:1,end:-1:1)); 12 | Q = Q'; 13 | Q = Q(end:-1:1,end:-1:1); 14 | U = U'; 15 | U = U(end:-1:1,end:-1:1); 16 | 17 | if det(Q)<0 18 | U(:,1) = -U(:,1); 19 | Q(1,:) = -Q(1,:); 20 | end 21 | 22 | return 23 | -------------------------------------------------------------------------------- /vgg_numerics/vgg_solvelin_blksym.m: -------------------------------------------------------------------------------- 1 | %VGG_SOLVELIN_BLKSYM Solves M*x==y where M is (typically huge sparse) symmetric 4-block matrix. 2 | % It solves the system much more efficiently than a general (sparse) linear system solver. 3 | % Typical usage to solve normal equations in Levenberg-Marquardt in bundle adjustment. 4 | % 5 | % X = VGG_SOLVELIN_BLKSYM(A,B,C,p,q [,'nocheck']) solves M*x==Y where 6 | % M=[A B; B' C] and y=[p;q]. 7 | % 8 | % X = VGG_SOLVELIN_BLKSYM(M,y,sideA [,'nocheck']) does the same but takes M and splits it, 9 | % M=[A B; B' C] and size(A)==[sideA sideA]. 10 | % 11 | % The function checks whether C is really near to diagonal; if not, a warning is printed. 12 | % Parmeter 'nocheck' switches off this test. 13 | 14 | % (c) {awf,werner}@robots.ox.ac.uk, March 2002 15 | 16 | 17 | function x = vgg_solvelin_blksym(varargin) 18 | 19 | switch nargin 20 | case 6 21 | [A,B,C,p,q,check] = deal(varargin{:}); 22 | case 5 23 | [A,B,C,p,q] = deal(varargin{:}); 24 | check = ''; 25 | case 4 26 | [M,y,sideA,check] = deal(varargin{:}); 27 | case 3 28 | [M,y,sideA] = deal(varargin{:}); 29 | check = ''; 30 | otherwise 31 | error('Bad number of parameters'); 32 | end 33 | 34 | if nargin<5 35 | Rtop = 1:sideA; 36 | Rbot = sideA+1:size(M,1); 37 | C = M(Rbot, Rbot); 38 | B = M(Rtop, Rbot); 39 | A = M(Rtop, Rtop); 40 | p = y(Rtop); 41 | q = y(Rbot); 42 | end 43 | 44 | if ~strcmp(check,'nocheck') & nnz(C(1,:))/size(C,1) > .1 45 | warning('It is likely that M was splitted incorrectly. Check diagonality of C and/or value of sideA.'); 46 | end 47 | 48 | %tic 49 | 50 | % Surprisingly, branch 1 is 2x slower than branch 2. We don't know why. 51 | switch 2 52 | case 1 53 | invC_Btq = C \ [B' q]; 54 | case 2 55 | invC_Btq = inv(C) * [B' q]; 56 | end 57 | 58 | invC_Bt = invC_Btq(:,1:end-1); 59 | invC_q = invC_Btq(:,end); 60 | 61 | u = (A - B * invC_Bt) \ (p - B * invC_q); 62 | v = invC_q - invC_Bt*u; 63 | 64 | x = [u;v]; 65 | 66 | %toc 67 | 68 | return -------------------------------------------------------------------------------- /vgg_numerics/vgg_vec.m: -------------------------------------------------------------------------------- 1 | % vgg_vec De-/vectorization of a matrix. 2 | % 3 | % For a matrix X, vgg_vec(X) = X(:). 4 | % For a N^2-vector x, vgg_vec(x) = reshape(x,N,N). 5 | % 6 | % Classical matrix re-arrangement operator, see book Magnus-Neudecker. 7 | % Trivial function, included mainly for consistency with notation in literature. 8 | % 9 | % Useful for rearranging matrix equations. Matrix from the middle of a product can be put to the right as 10 | % 11 | % vgg_vec(A*B*C) = kron(C',A)*vgg_vec(B) 12 | % 13 | % See also vgg_vec_swap, vgg_commut_matrix, vgg_duplic_matrix. 14 | 15 | function v = vgg_vec(A) 16 | 17 | if any(size(A)==1) 18 | v = reshape(A,[1 1]*sqrt(prod(size(A)))); 19 | else 20 | v = A(:); 21 | end 22 | 23 | return -------------------------------------------------------------------------------- /vgg_numerics/vgg_vec_swap.m: -------------------------------------------------------------------------------- 1 | % S = vgg_vec_swap(x,y) For square matrix A and vectors x, y it is x'*A*y = vgg_vec(A)'*vgg_vec_swap(x,y). 2 | % 3 | % x ... matrix N-by-K 4 | % y ... matrix N-by-K 5 | % S ... matrix K^2-by-N 6 | % 7 | % Examples :- 8 | % 9 | % - Estimating fundamental matrix F that should satisfy x(:,k)'*F*y(:,k) = 0 from 10 | % given points x(:,k), y(:,k). We solve homogeneous system vec(F)*vgg_vec_swap(x,y) = 0. 11 | % 12 | % - Evaluating values v(k) of x(:,k)'*A{k}*y(:,k) for all k, A{k} are square matrices. 13 | % If column vectors vec(A{k}) are stacked to a single matrix AA, then 14 | % v = sum(AA'.*vgg_vec_swap(x,y)). 15 | % 16 | % See also vgg_vech_swap 17 | 18 | % Tomas Werner, Oct 2001 19 | 20 | function M = vgg_vec_swap(x,y) 21 | 22 | if nargin==1 23 | y = x; 24 | end 25 | 26 | [i j] = find(ones(size(x,1))); 27 | M = x(i,:).*y(j,:); 28 | 29 | return 30 | 31 | -------------------------------------------------------------------------------- /vgg_numerics/vgg_vech.m: -------------------------------------------------------------------------------- 1 | function h = vgg_vech(m) 2 | % VGG_VECH Vectorization ("flattening") of symmetric matrix. 3 | % 4 | % For square matrix X, vgg_vech(X) is the column vector of elements on or 5 | % below the main diagonal of m. 6 | % 7 | % Also works inversely: for a N*(N+1)/2-vector x, it returns symmetric 8 | % N-by-N matrix X = vgg_vech(x) such that vgg_vech(X) = x. 9 | % 10 | % Useful for solving linear matrix equations, see Magnus and Neudecker. 11 | % 12 | % See vgg_matrix_test, vgg_duplic_matrix, and also vgg_vech_swap, 13 | % vgg_commut_matrix. 14 | 15 | [M N] = size(m); 16 | 17 | if M==1 | N==1 18 | N = (sqrt(8*M*N+1)-1)/2; 19 | r = (1:N)'*ones(1,N); 20 | c = r'; 21 | h = zeros(N); 22 | h(find(c <= r)) = m; 23 | h = h+h'-diag(diag(h)); 24 | else 25 | r = (1:M)'*ones(1,N); 26 | c = ones(M,1)*(1:N); 27 | h = m(find(c <= r)); 28 | end 29 | 30 | return -------------------------------------------------------------------------------- /vgg_numerics/vgg_vech_swap.m: -------------------------------------------------------------------------------- 1 | % S = vgg_vech_swap(x,y) For symmetric matrix A it is x'*A*y = vgg_vech(A)'*vgg_vech_swap(x,y). 2 | % 3 | % x ... matrix N-by-K 4 | % y ... matrix N-by-K 5 | % S ... matrix K*(K+1)/2-by-N 6 | % 7 | % Examples :- 8 | % 9 | % - Estimating symmetric matrix A that should satisfy x(:,k)'*A*y(:,k) = 0 from 10 | % given points x(:,k), y(:,k). We solve homogeneous system vech(A)*vgg_vec_swap(x,y) = 0. 11 | % 12 | % - Evaluating values v(k) of x(:,k)'*A{k}*y(:,k) for all k, A{k} are square symmetric matrices. 13 | % If column vectors vech(A{k}) are stacked to a single matrix AA, then 14 | % v = sum(AA'.*vgg_vech_swap(x,y)). 15 | % 16 | % See also vgg_matrix_test, vgg_lineseg_from_x, vgg_vec_swap, vgg_commut_matrix, vgg_duplic_matrix. 17 | 18 | % Tomas Werner, Oct 2001 19 | 20 | function M = vgg_vech_swap(x,y) 21 | 22 | if nargin==1 23 | y = x; 24 | end 25 | 26 | [i j] = find(ones(size(x,1))); 27 | d = i>=j; 28 | i = i(d); 29 | j = j(d); 30 | 31 | M = x(i,:).*y(j,:) + x(j,:).*y(i,:).*((i~=j)*ones(1,size(x,2))); 32 | 33 | return -------------------------------------------------------------------------------- /vgg_numerics/vgg_wedge.m: -------------------------------------------------------------------------------- 1 | % vgg_wedge Wedge product of N-1 N-vectors (generalization of cross product). 2 | % 3 | % Y = vgg_wedge(X) Wedge product of columns/rows of X. 4 | % Y ... double (1,N). 5 | % X ... double (N,N-1). 6 | % It is Y = X(:,1) \wedge X(:,2) \wedge ... \wedge X(:,N-1). For N=3, 7 | % wedge product is the same as cross (vector) product. E.g., for N=4, 8 | % wedge product of three 4-vectors is in fact computation of a plane in 9 | % 3-D projective space from 3 points in the plane. 10 | % The sign is chosen so that for any square matrix X it is: det(X) == wedge(X(:,1:end-1))*X(:,end) 11 | % Works also dually for 12 | % Y ... double (N,1). 13 | % X ... double (N-1,N). 14 | % 15 | % Y = vgg_wedge(X_1,X_2,...,X_{N-1}) Wedge product for each (N-1)-tuple of corresponding columns of X_n. 16 | % X_n ... double (N,K) 17 | % Y ... double (K,N) 18 | % Equivalent to 19 | % for k = 1:K, Y(k,:) = wedge([X_1(:,k) ... X_{N-1}(:,k)]); end 20 | % E.g.: wedge(X1,X2) is the same as cross(X1,X2)' but faster. 21 | % Dual form is not available. 22 | 23 | function Y = vgg_wedge(varargin) 24 | 25 | if nargin == 1 26 | 27 | X = varargin{1}; 28 | 29 | [N,Nm1] = size(X); 30 | if Nm1>N 31 | Y = vgg_wedge(X')'; 32 | return 33 | end 34 | 35 | switch N 36 | case 3 % make it faster for special case N==3 37 | Y = [X(2,1).*X(3,2)-X(3,1).*X(2,2),... 38 | X(3,1).*X(1,2)-X(1,1).*X(3,2),... 39 | X(1,1).*X(2,2)-X(2,1).*X(1,2)]; 40 | otherwise 41 | for n = 1:N 42 | Y(n) = (-1)^(n+N)*det(X([1:n-1 n+1:N],:)); 43 | end 44 | end 45 | 46 | else 47 | 48 | N = nargin + 1; 49 | switch N 50 | case 3 % make it faster for special case N==3 51 | X1 = varargin{1}; 52 | X2 = varargin{2}; 53 | Y = [(X1(2,:).*X2(3,:)-X1(3,:).*X2(2,:))',... 54 | (X1(3,:).*X2(1,:)-X1(1,:).*X2(3,:))',... 55 | (X1(1,:).*X2(2,:)-X1(2,:).*X2(1,:))']; 56 | otherwise 57 | for k = 1:size(varargin{1},2) 58 | for n = 1:N-1 59 | X(:,n) = varargin{n}(:,k); 60 | end 61 | Y(k,:) = vgg_wedge(X); 62 | end 63 | end 64 | 65 | end 66 | 67 | 68 | return -------------------------------------------------------------------------------- /vgg_ui/Contents.m: -------------------------------------------------------------------------------- 1 | % VGG User Interface Library 2 | % 3 | % GUI's 4 | % vgg_gui_F - Visualizes epipolar geometry between two views 5 | % vgg_gui_H - Visualizes a homography between two views 6 | -------------------------------------------------------------------------------- /vgg_ui/vgg_gui_F.m: -------------------------------------------------------------------------------- 1 | function fig=vgg_gui_F(i1,i2,F) 2 | % 3 | % fig=vgg_gui_F(i1,i2,F) 4 | % 5 | % 6 | % Visualizes the fundamental matrix of two views 7 | % 8 | %IN: 9 | % i1 - Matlab image 10 | % i2 - Matlab image 11 | % F - Fundamental matrix (p1'*F*p2=0). Assumes that image coordiantes 12 | % are 1..width where pixel centers are at integer locations. 13 | % 14 | %OUT: 15 | % fig - handle to the figure 16 | 17 | % $Id: vgg_gui_F.m,v 1.8 2002/05/20 21:35:42 wexler Exp $ 18 | % Yoni, Tue Mar 27 19:31:16 2001 19 | 20 | 21 | if nargin==3 22 | action='start'; 23 | else 24 | action=i1; 25 | ud = get(gcf, 'UserData'); 26 | end 27 | 28 | if strcmp(action,'start'), 29 | if nargin ~= 3 30 | error('Must give 3 arguments... read the docs.\n'); 31 | end 32 | 33 | h0 = figure('Color',[0.8 0.8 0.8], ... 34 | 'NumberTitle','off', ... 35 | 'Name','Play With Fundamental Matrix', ... 36 | 'ButtonDownFcn', 'disp(''Click on images'')',... 37 | 'WindowButtonUpFcn', 'vgg_gui_F(''none'');',... 38 | 'WindowButtonMotionFcn', 'vgg_gui_F(''move'')', ... 39 | 'Pointer', 'crosshair', ... 40 | 'DoubleBuffer', 'on',... 41 | 'Units','normalized'); 42 | [pointerShape, pointerHotSpot] = CreatePointer; 43 | set(h0, 'Pointer', 'custom', ... 44 | 'PointerShapeCData', pointerShape, ... 45 | 'PointerShapeHotSpot', pointerHotSpot); 46 | 47 | m0=uimenu('Label', '&Color'); 48 | uimenu(m0, 'Label', 'blac&K', 'ForegroundColor', [0 0 0], ... 49 | 'Accelerator', 'k', 'Callback', 'vgg_gui_F(''ck'');'); 50 | uimenu(m0, 'Label', '&Red', 'ForegroundColor', [1 0 0], ... 51 | 'Accelerator', 'r', 'Callback', 'vgg_gui_F(''cr'');'); 52 | uimenu(m0, 'Label', '&Green', 'ForegroundColor', [0 1 0], ... 53 | 'Accelerator', 'g', 'Callback', 'vgg_gui_F(''cg'');'); 54 | uimenu(m0, 'Label', '&Blue', 'ForegroundColor', [0 0 1], ... 55 | 'Accelerator', 'b', 'Callback', 'vgg_gui_F(''cb'');'); 56 | m1=uimenu('Label', '&Size'); 57 | uimenu(m1, 'Label', '&Increase', 'Callback', 'vgg_gui_F(''s+'');', 'Accelerator', '+'); 58 | uimenu(m1, 'Label', '&Decrease', 'Callback', 'vgg_gui_F(''s-'');', 'Accelerator', '-'); 59 | uimenu(m1, 'Label', '&1', 'Callback', 'vgg_gui_F(''s1'');', 'Accelerator', '1'); 60 | uimenu(m1, 'Label', '&2', 'Callback', 'vgg_gui_F(''s2'');', 'Accelerator', '2'); 61 | uimenu(m1, 'Label', '&3', 'Callback', 'vgg_gui_F(''s3'');', 'Accelerator', '3'); 62 | uimenu(m1, 'Label', '&4', 'Callback', 'vgg_gui_F(''s4'');', 'Accelerator', '4'); 63 | uimenu(m1, 'Label', '&5', 'Callback', 'vgg_gui_F(''s5'');', 'Accelerator', '5'); 64 | uimenu(m1, 'Label', '&6', 'Callback', 'vgg_gui_F(''s6'');', 'Accelerator', '6'); 65 | uimenu(m1, 'Label', '&7', 'Callback', 'vgg_gui_F(''s7'');', 'Accelerator', '7'); 66 | uimenu(m1, 'Label', '&8', 'Callback', 'vgg_gui_F(''s8'');', 'Accelerator', '8'); 67 | uimenu(m1, 'Label', '&9', 'Callback', 'vgg_gui_F(''s9'');', 'Accelerator', '9'); 68 | 69 | ah1 = axes('Parent', h0, ... 70 | 'Position',[0 0 .5 1]); 71 | h1=imshow(i1); hold on; title('Image 1'); 72 | set(h1, 'ButtonDownFcn','vgg_gui_F(''b1'');'); 73 | 74 | ah2 = axes('Parent',h0, ... 75 | 'Position',[.5 0 .5 1], ... 76 | 'Tag','Axes2'); 77 | h2=imshow(i2); hold on; title('Image 2'); 78 | set(h2, 'ButtonDownFcn','vgg_gui_F(''b2'');'); 79 | 80 | point=plot(-1000, -1000,'EraseMode','xor'); 81 | l=plot([-1000, -1001], [-1000 -1000], 'r-','EraseMode','xor'); 82 | 83 | s1=size(i1); s2=size(i2); 84 | t(:,:,1)=F'; t(:,:,2)=F; F=t; 85 | 86 | ud=struct('h0', h0, 'h',[h1 h2], 'ah', [ah1, ah2], ... 87 | 'sizes', [s1(1:2); s2(1:2)], ... 88 | 'current', -1, 'color', 'k', 'size', 1, ... 89 | 'p', point, 'F', F, 'l', l ); 90 | 91 | set(h0,'UserData',ud); 92 | 93 | if nargout > 0, fig = h0; end 94 | elseif strcmp(action, 'move') 95 | if ud.current<0 return; end; 96 | pt=get(ud.ah(ud.current),'CurrentPoint'); 97 | pts=get_line_points(ud.F(:,:,ud.current)*pt(1,:)', ... 98 | ud.sizes(ud.current,:)); 99 | 100 | set(ud.l, 'XData', pts(1,:), 'YData', pts(2,:)) 101 | set(ud.p, 'XData', pt(1,1), 'YData', pt(1,2)) 102 | 103 | elseif action(1)=='b' 104 | if action(2)=='1' ud.current=1; 105 | elseif action(2)=='2' ud.current=2; 106 | else return; 107 | end 108 | pt=get(ud.ah(ud.current),'CurrentPoint'); 109 | 110 | pts=get_line_points(ud.F(:,:,ud.current)*pt(1,:)', ud.sizes(1,:)); 111 | delete(ud.p); 112 | delete(ud.l); 113 | axes(ud.ah(ud.current)); 114 | ud.p=plot(pt(1,1), pt(1,2), [ud.color '+'], ... 115 | 'MarkerSize', 8+2*ud.size, 'LineWidth', ud.size,... 116 | 'EraseMode','xor'); 117 | axes(ud.ah(3-ud.current)); 118 | ud.l=plot(pts(1,:), pts(2,:), [ud.color '-'], ... 119 | 'LineWidth', ud.size, 'EraseMode','xor'); 120 | elseif action(1)=='c' 121 | ud.color=action(2); 122 | %get(ud.l) 123 | set(ud.l, 'Color', ud.color); 124 | set(ud.p, 'Color', ud.color); 125 | elseif action(1)=='s' 126 | if action(2)=='+' ud.size=ud.size+1; 127 | elseif action(2)=='-' ud.size=max(1, ud.size-1); 128 | else 129 | ud.size = str2num(action(2)); 130 | end 131 | set(ud.p, 'LineWidth', ud.size, 'MarkerSize', 8+2*ud.size); 132 | set(ud.l, 'LineWidth', ud.size, 'MarkerSize', 8+2*ud.size); 133 | elseif strcmp(action, 'none') 134 | ud.current = -1; 135 | else 136 | error(['Unknown command: ' action]); 137 | end 138 | 139 | set(ud.h0, 'UserData',ud); 140 | 141 | 142 | 143 | function pts=get_line_points(l,sz) 144 | a=l(1); b=l(2);c=l(3); 145 | h=sz(1); w=sz(2); 146 | 147 | % This might cause 'divide by zero' warning: 148 | ys=c/-b ; 149 | yf=-(a*w+c)/b; 150 | xs=c/-a; 151 | xf=-(b*h+c)/a; 152 | 153 | m1 = [[xs;1] [xf;h] [1;ys] [w;yf]]; 154 | w2 = [(xs<=w & xs>=1) (xf<=w & xf>=1) (ys<=h & ys>=1) (yf<=h & yf>=1)]; 155 | v = w2>0; 156 | pts = [m1(:,v)]; 157 | 158 | 159 | 160 | % Taken from pixval.m: 161 | function [pointerShape, pointerHotSpot] = CreatePointer 162 | 163 | pointerHotSpot = [8 8]; 164 | pointerShape = [ ... 165 | NaN NaN NaN NaN NaN 1 2 NaN 2 1 NaN NaN NaN NaN NaN NaN 166 | NaN NaN NaN NaN NaN 1 2 NaN 2 1 NaN NaN NaN NaN NaN NaN 167 | NaN NaN NaN NaN NaN 1 2 NaN 2 1 NaN NaN NaN NaN NaN NaN 168 | NaN NaN NaN NaN NaN 1 2 NaN 2 1 NaN NaN NaN NaN NaN NaN 169 | NaN NaN NaN NaN NaN 1 2 NaN 2 1 NaN NaN NaN NaN NaN NaN 170 | 1 1 1 1 1 1 2 NaN 2 1 1 1 1 1 1 1 171 | 2 2 2 2 2 2 NaN NaN NaN 2 2 2 2 2 2 2 172 | NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN 173 | 2 2 2 2 2 2 NaN NaN NaN 2 2 2 2 2 2 2 174 | 1 1 1 1 1 1 2 NaN 2 1 1 1 1 1 1 1 175 | NaN NaN NaN NaN NaN 1 2 NaN 2 1 NaN NaN NaN NaN NaN NaN 176 | NaN NaN NaN NaN NaN 1 2 NaN 2 1 NaN NaN NaN NaN NaN NaN 177 | NaN NaN NaN NaN NaN 1 2 NaN 2 1 NaN NaN NaN NaN NaN NaN 178 | NaN NaN NaN NaN NaN 1 2 NaN 2 1 NaN NaN NaN NaN NaN NaN 179 | NaN NaN NaN NaN NaN 1 2 NaN 2 1 NaN NaN NaN NaN NaN NaN 180 | NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN]; 181 | -------------------------------------------------------------------------------- /vgg_ui/vgg_gui_H.m: -------------------------------------------------------------------------------- 1 | function fig=vgg_gui_H(i1,i2,H) 2 | % 3 | % fig=vgg_gui_H(i1,i2,H) 4 | % 5 | % 6 | % Visualizes an homography matrix of two views 7 | % 8 | %IN: 9 | % i1 - Matlab image 10 | % i2 - Matlab image 11 | % H - 3x3 Homography matrix. Assumes that image coordiantes are 1..width 12 | % where pixel centers are at integer locations. 13 | % 14 | %OUT: 15 | % fig - handle to the figure 16 | 17 | % $Id: vgg_gui_H.m,v 1.3 2001/10/30 14:42:41 wexler Exp $ 18 | % Yoni, Tue Mar 27 19:31:16 2001 19 | 20 | 21 | if nargin==3 22 | action='start'; 23 | else 24 | action=i1; 25 | ud = get(gcf, 'UserData'); 26 | end 27 | 28 | if strcmp(action,'start'), 29 | if nargin ~= 3 30 | error('Must give 3 arguments... read the docs.\n'); 31 | end 32 | 33 | h0 = figure('Color',[0.8 0.8 0.8], ... 34 | 'NumberTitle','off', ... 35 | 'Name','Play With Homography Matrix', ... 36 | 'ButtonDownFcn', 'disp(''Click on images'')',... 37 | 'WindowButtonUpFcn', 'vgg_gui_H(''none'');',... 38 | 'WindowButtonMotionFcn', 'vgg_gui_H(''move'')', ... 39 | 'Pointer', 'crosshair', ... 40 | 'DoubleBuffer', 'on',... 41 | 'Units','normalized'); 42 | m0=uimenu('Label', '&Color'); 43 | uimenu(m0, 'Label', '&Red', 'ForegroundColor', [1 0 0], ... 44 | 'Accelerator', 'r', 'Callback', 'vgg_gui_H(''cr'');'); 45 | uimenu(m0, 'Label', '&Green', 'ForegroundColor', [0 1 0], ... 46 | 'Accelerator', 'g', 'Callback', 'vgg_gui_H(''cg'');'); 47 | uimenu(m0, 'Label', '&Blue', 'ForegroundColor', [0 0 1], ... 48 | 'Accelerator', 'b', 'Callback', 'vgg_gui_H(''cb'');'); 49 | m1=uimenu('Label', '&Size'); 50 | uimenu(m1, 'Label', '&Increase', 'Callback', 'vgg_gui_H(''s+'');', 'Accelerator', '+'); 51 | uimenu(m1, 'Label', '&Decrease', 'Callback', 'vgg_gui_H(''s-'');', 'Accelerator', '-'); 52 | uimenu(m1, 'Label', '&1', 'Callback', 'vgg_gui_H(''s1'');', 'Accelerator', '1'); 53 | uimenu(m1, 'Label', '&2', 'Callback', 'vgg_gui_H(''s2'');', 'Accelerator', '2'); 54 | uimenu(m1, 'Label', '&3', 'Callback', 'vgg_gui_H(''s3'');', 'Accelerator', '3'); 55 | uimenu(m1, 'Label', '&4', 'Callback', 'vgg_gui_H(''s4'');', 'Accelerator', '4'); 56 | uimenu(m1, 'Label', '&5', 'Callback', 'vgg_gui_H(''s5'');', 'Accelerator', '5'); 57 | uimenu(m1, 'Label', '&6', 'Callback', 'vgg_gui_H(''s6'');', 'Accelerator', '6'); 58 | uimenu(m1, 'Label', '&7', 'Callback', 'vgg_gui_H(''s7'');', 'Accelerator', '7'); 59 | uimenu(m1, 'Label', '&8', 'Callback', 'vgg_gui_H(''s8'');', 'Accelerator', '8'); 60 | uimenu(m1, 'Label', '&9', 'Callback', 'vgg_gui_H(''s9'');', 'Accelerator', '9'); 61 | 62 | ah1 = axes('Parent', h0, ... 63 | 'Position',[0 0 .5 1]); 64 | h1=imshow(i1); hold on; title('Image 1'); 65 | set(h1, 'ButtonDownFcn','vgg_gui_H(''b1'');'); 66 | 67 | ah2 = axes('Parent',h0, ... 68 | 'Position',[.5 0 .5 1], ... 69 | 'Tag','Axes2'); 70 | h2=imshow(i2); hold on; title('Image 2'); 71 | set(h2, 'ButtonDownFcn','vgg_gui_H(''b2'');'); 72 | 73 | point=plot(-1000, -1000,'EraseMode','xor'); 74 | point2=plot(-1000, -1000,'EraseMode','xor'); 75 | 76 | s1=size(i1); s2=size(i2); 77 | t(:,:,1)=H; t(:,:,2)=inv(H); H=t; 78 | 79 | ud=struct('h0', h0, 'h',[h1 h2], 'ah', [ah1, ah2], ... 80 | 'sizes', [s1(1:2); s2(1:2)], ... 81 | 'current', -1, 'color', 'r', 'size', 1, ... 82 | 'p', point, 'H', H, 'p2', point2 ); 83 | 84 | set(h0,'UserData',ud); 85 | 86 | if nargout > 0, fig = h0; end 87 | 88 | elseif strcmp(action, 'move') 89 | if ud.current<0 return; end; 90 | pt=get(ud.ah(ud.current),'CurrentPoint'); 91 | pt2=ud.H(:,:,ud.current)*pt(1,:)'; 92 | pt2=pt2/pt2(3); 93 | 94 | set(ud.p2, 'XData', pt2(1,1), 'YData', pt2(2,1)) 95 | set(ud.p, 'XData', pt(1,1), 'YData', pt(1,2)) 96 | 97 | elseif action(1)=='b' 98 | if action(2)=='1' ud.current=1; 99 | elseif action(2)=='2' ud.current=2; 100 | else return; 101 | end 102 | pt=get(ud.ah(ud.current),'CurrentPoint'); 103 | 104 | p2=ud.H(:,:,ud.current)*pt(1,:)'; 105 | p2=p2/p2(3); 106 | 107 | delete(ud.p); 108 | delete(ud.p2); 109 | axes(ud.ah(ud.current)); 110 | ud.p=plot(pt(1,1), pt(1,2), [ud.color '+'], ... 111 | 'MarkerSize', 8+2*ud.size, 'LineWidth', ud.size,... 112 | 'EraseMode','xor'); 113 | axes(ud.ah(3-ud.current)); 114 | ud.p2=plot(p2(1,1), p2(2,1), [ud.color '+'], ... 115 | 'MarkerSize', 8+2*ud.size, 'LineWidth', ud.size,... 116 | 'EraseMode','xor'); 117 | 118 | elseif action(1)=='c' 119 | ud.color=action(2); 120 | %get(ud.l) 121 | set(ud.p2, 'Color', ud.color); 122 | set(ud.p, 'Color', ud.color); 123 | 124 | elseif action(1)=='s' 125 | if action(2)=='+' ud.size=ud.size+1; 126 | elseif action(2)=='-' ud.size=max(1, ud.size-1); 127 | else 128 | ud.size = str2num(action(2)); 129 | end 130 | set(ud.p, 'LineWidth', ud.size, 'MarkerSize', 8+2*ud.size); 131 | set(ud.p2, 'LineWidth', ud.size, 'MarkerSize', 8+2*ud.size); 132 | 133 | elseif strcmp(action, 'none') 134 | ud.current = -1; 135 | 136 | else 137 | error(['Unknown command: ' action]); 138 | end 139 | 140 | set(ud.h0, 'UserData',ud); 141 | 142 | --------------------------------------------------------------------------------