% RQ3 RQ decomposition of 3x3 matrix % % Usage: [R,Q] = rq3(A) % % Argument: A - 3 x 3 matrix % Returns: R - Upper triangular 3 x 3 matrix % Q - 3 x 3 orthonormal rotation matrix % Such that R*Q = A % % The signs of the rows and columns of R and Q are chosen so that the diagonal % elements of R are +ve. % % See also: DECOMPOSECAMERA % Follows algorithm given by Hartley and Zisserman 2nd Ed. A4.1 p 579 % Copyright (c) 2010 Peter Kovesi % Centre for Exploration Targeting % School of Earth and Environment % The University of Western Australia % peter.kovesi at uwa edu au % % 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, subject to the following conditions: % % The above copyright notice and this permission notice shall be included in % all copies or substantial portions of the Software. % % October 2010 % February 2014 Incorporated modifications suggested by Mathias Rothermel to % avoid potential division by zero problems function [R,Q] = rq3(A) if ~all(size(A)==[3 3]) error('A must be 3x3'); end eps = 1e-10; % Find rotation Qx to set A(3,2) to 0 A(3,3) = A(3,3) + eps; c = -A(3,3)/sqrt(A(3,3)^2+A(3,2)^2); s = A(3,2)/sqrt(A(3,3)^2+A(3,2)^2); Qx = [1 0 0; 0 c -s; 0 s c]; R = A*Qx; % Find rotation Qy to set A(3,1) to 0 R(3,3) = R(3,3) + eps; c = R(3,3)/sqrt(R(3,3)^2+R(3,1)^2); s = R(3,1)/sqrt(R(3,3)^2+R(3,1)^2); Qy = [c 0 s; 0 1 0;-s 0 c]; R = R*Qy; % Find rotation Qz to set A(2,1) to 0 R(2,2) = R(2,2) + eps; c = -R(2,2)/sqrt(R(2,2)^2+R(2,1)^2); s = R(2,1)/sqrt(R(2,2)^2+R(2,1)^2); Qz = [c -s 0; s c 0; 0 0 1]; R = R*Qz; Q = Qz'*Qy'*Qx'; % Adjust R and Q so that the diagonal elements of R are +ve for n = 1:3 if R(n,n) < 0 R(:,n) = -R(:,n); Q(n,:) = -Q(n,:); end end