gusucode.com > vision工具箱matlab源码程序 > vision/+vision/+internal/+calibration/solveP3P.m
% solveP3P World camera pose from three 2D-3D correspondences % [Rs, Ts] = solveP3P(imagePoints, worldPoints, K) returns up to four % solutions to the perspective-3-point problem. imagePoints is an M-by-2 % array containing [x,y] image point coordinates. worldPoints is an M-by-3 % array containing the corresponding [x,y,z] world coordinates. K is the % 3-by-3 intrinsics matrix. The function assumes no lens distortion. % % Rs is a 3-by-3-by-P array containing the rotation matrices, and Ts is a % P-by-3 matrix containing the translation vectors, where 1 <= P <= 4. % % Note: if the 4th degree polynomial has no real solution, Rs and Ts will % be empty. % Refrences: % X.-Sh. Gao, X.-R. Hou, J. Tang, H.-F. Cheng. Complete Solution % Classification for the Perspective-Three-Point Problem. IEEE PAMI, % v. 25(8), 2003. pp. 930-943. % % The P3P (Perspective-Three-Point) Principle % http://iplimage.com/blog/p3p-perspective-point-overview/ % Copyright 2016 MathWorks, Inc. %#codegen function [Rs, Ts] = solveP3P(imagePointsIn, worldPointsIn, K) % The computation is done in double precision internally imagePoints = double(imagePointsIn); worldPoints = double(worldPointsIn); % Convert image points to normalized image coordinates numPoints = size(imagePoints, 1); U = [imagePoints, ones(numPoints, 1)] / K; % Normalize to unit vectors uNorm = sqrt(sum(U.^2, 2)); U = bsxfun(@rdivide, U, uNorm); % u, v, and w are unit vectors, which point to the world points from the % camera center. u = U(1, :); v = U(2, :); w = U(3, :); % Angles between the unit vectors cosUV = u * v'; cosUW = u * w'; cosVW = v * w'; % World points A = worldPoints(1, :); B = worldPoints(2, :); C = worldPoints(3, :); % Squared distances between the world points AB2 = sum((A - B).^2); BC2 = sum((B - C).^2); AC2 = sum((A - C).^2); a = BC2 / AB2; b = AC2 / AB2; p = 2 * cosVW; q = 2 * cosUW; r = 2 * cosUV; % Check that we do not have a "degenerate" case c = whichComponent(a, b, p, q, r); if c ~= 1 % Output poses Rs = zeros(3, 3, 0, 'like', imagePointsIn); Ts = zeros(0, 3, 'like', imagePointsIn); return; end % Coefficients of the 4th degree polynomial equation pbr = p*b*r; a4 = -2*b + b^2 + a^2 + 1 - b*r^2*a + 2*b*a - 2*a; a3 = -2*b*q*a - 2*a^2*q + b*r^2*q*a - 2*q + 2*b*q + 4*a*q + pbr + pbr*a - b^2*r*p; a2 = q^2 + b^2*r^2 - b*p^2 - q*pbr + b^2*p^2 - b*r^2*a + 2 - 2*b^2 - a*pbr*q + 2*a^2 - 4*a - 2*q^2*a + q^2*a^2; a1 = -b^2*r*p + pbr*a - 2*a^2*q + q*p^2*b + 2*b*q*a + 4*a*q + pbr - 2*b*q - 2*q; a0 = 1 - 2*a + 2*b + b^2 - b*p^2 + a^2 - 2*b*a; % Solve the 4th degree polynomial equation coeffs = [a4, a3, a2, a1, a0]; if ~all(isfinite(coeffs)) Rs = zeros(3, 3, 0, 'like', imagePointsIn); Ts = zeros(0, 3, 'like', imagePointsIn); return; end X = roots(coeffs); % Only keep the real roots X = real(X(abs(imag(X)) < 1e-8)); numSolutions = numel(X); % Output poses Rs = zeros([3, 3, numSolutions], 'like', imagePointsIn); Ts = zeros([numSolutions, 3], 'like', imagePointsIn); % Coefficients of the second equation b1 = b*((p^2 - p*q*r + r^2)*a + (p^2 - r^2)*b - p^2 + p*q*r - r^2)^2; % Compute output poses for i = 1:numSolutions x = X(i); b0 = ((1 - a - b)*x^2 + (a - 1)*q*x - a + b + 1) * ... ((r^3 * (a^2 + b^2 - 2*a - 2*b + (2 - r^2)*a*b + 1))*x^3 + ... r^2*(p+p*a^2 - 2*r*q*a*b + 2*r*q*b - 2*r*q - 2*p*a - 2*p*b + p*r^2*b + 4*r*q*a + q*r^3*a*b - 2*r*q*a^2 + 2*p*a*b + p*b^2 - r^2*p*b^2)*x^2 + ... (r^5*(b^2 - a*b) - r^4*p*q*b + r^3*(q^2 - 4*a - 2*q^2*a + q^2*a^2 + 2*a^2 - 2*b^2 + 2) + r^2*(4*p*q*a - 2*p*q*a*b + 2*p*q*b - 2*p*q - 2*p*q*a^2) + ... r*(p^2*b^2 - 2*p^2*b + 2*p^2*a*b - 2*p^2*a + p^2 + p^2*a^2))*x + ... (2*p*r^2 - 2*r^3*q + p^3 - 2*p^2*q*r + p*q^2*r^2)*a^2 + (p^3 - 2*p*r^2)*b^2 + (4*q*r^3 - 4*p*r^2 - 2*p^3 + 4*p^2*q*r - 2*p*q^2*r^2)*a + ... (-2*q*r^3 + p*r^4 + 2*p^2*q*r - 2*p^3)*b + (2*p^3 + 2*q*r^3 - 2*p^2*q*r)*a*b + ... p*q^2*r^2 - 2*p^2*q*r + 2*p*r^2 + p^3 - 2*r^3*q); y = b0 / b1; cv = x^2 + y^2 - 2*x*y*cosUV; % Distances from the camera center to the world points PC = sqrt(AB2 / cv); PB = y * PC; PA = x * PC; % The distances must be positive if PC > 0 && PB > 0 && PA > 0 % Find coordinates of the world points in the camera's coordinate % system Ac = u * PA; Bc = v * PB; Cc = w * PC; % Compute the rigid transformation from world into camera % coordinate system [R, t] = vision.internal.calibration.rigidTransform3D(... worldPoints(1:3, :), [Ac; Bc; Cc;]); Rs(:,:,i) = cast(R', 'like', imagePointsIn); Ts(i, :) = cast(t', 'like', imagePointsIn); end end function c = whichComponent(a, b, p, q, r) if isTS2(a, b, r) c = 2; elseif isTS3(a, b, p, q, r) c = 3; elseif isTS4(a, b, r) c = 4; elseif isTS5(a, b, p, q, r) c = 5; elseif isTS6(a, b, p, q, r) c = 6; elseif isTS7(b, p, q, r) c = 7; elseif isTS8() c = 8; elseif isTS9(p, r) c = 9; % elseif isTS10(a, b, p, q, r) % c = 10; % elseif isTS11(a, b, p, q, r) % c = 11; else c = 1; end %-------------------------------------------------------------------------- function tf = isTS2(a, b, r) tf = a^2 + (-2 + 2*b - b*r^2)*a - 2*b + b^2 + 1 == 0; %-------------------------------------------------------------------------- function tf = isTS3(a, b, p, q, r) pqr = p * q * r; F = (-4*p^2 + 4*pqr + r^2*p^2 + r^2*q^2 - r^3*p*q - 4*q^2); tf = (F*a + r^2*p^2 - 4*pqr + 4*q^2 == 0) && ... (F*b + r^2*q^2 + 4*p^2 - 4*pqr == 0); %-------------------------------------------------------------------------- function tf = isTS4(a, b, r) tf = (a + b - 1 == 0) && (r == 0); %-------------------------------------------------------------------------- function tf = isTS5(a, b, p, r, q) q2 = q^2; p2 = p^2; F = (p2 + q2); tf = (F*a - q2 == 0) && (F*b - p2 == 0) && (r == 0); %-------------------------------------------------------------------------- function tf = isTS6(a, b, p, r, q) q2 = q^2; p2 = p^2; F = (p2^2 - 2*p2*q2 + q2^2); tf = (F*a - p2*q2 - q2^2 == 0) && (F*b - p2*q2 - p2^2 == 0) && ... ((p2 + q2)*r - 4*p*q == 0); %-------------------------------------------------------------------------- function tf = isTS7(b, p, r, q) tf = (4*r^2 + p^2*q^2 + p^4 - r^4 - p^3*q*r + p*r^3*q - 4*q*p*r)*b + ... 2*p*r^3 - 2*p^2*r^2 + 2*p^3*q*r - p^2*q^2*r^2 - p^4 - r^4 == 0; %-------------------------------------------------------------------------- function tf = isTS8() % No way to tell tf = false; %-------------------------------------------------------------------------- function tf = isTS9(p, r) tf = (p == 0) && (r == 0);