rrs_3point

 RRS_3POINT Direct solution for the spatial resection from three points

 [R,Z] = rrs_3point(X, m)

 Input:
  X        - 3x3-matrix, rows are scene point coordinates
  m        - 3x3-matrix, rows are directons in the camera system

 Output:
  R      - Mx3x3-Matrix, M solution for the rotation from the scene into
                         the camera system
  Z      - Mx3-Vektor, M solutions for the projection centre 
           We have m_i ~ R_j (X_i - Z_j)


 Autor: Christian Beder

 sides of scene triangle and angles between camera rays

0001 %
0002 % RRS_3POINT Direct solution for the spatial resection from three points
0003 %
0004 % [R,Z] = rrs_3point(X, m)
0005 %
0006 % Input:
0007 %  X        - 3x3-matrix, rows are scene point coordinates
0008 %  m        - 3x3-matrix, rows are directons in the camera system
0009 %
0010 % Output:
0011 %  R      - Mx3x3-Matrix, M solution for the rotation from the scene into
0012 %                         the camera system
0013 %  Z      - Mx3-Vektor, M solutions for the projection centre
0014 %           We have m_i ~ R_j (X_i - Z_j)
0015 %
0016 %
0017 % Autor: Christian Beder
0018 %
0019 function [R,Z] = rrs_3point(X, m)
0020 % sides of scene triangle and angles between camera rays
0021 a = norm(X(3,:) - X(2,:));
0022 alpha = acos(m(3,:) * m(2,:)');
0023 b = norm(X(1,:) - X(3,:));
0024 beta = acos(m(1,:) * m(3,:)');
0025 c = norm(X(2,:) - X(1,:));
0026 gamma = acos(m(2,:) * m(1,:)');
0027 % coefficients
0028 A4 = ((a^2 - c^2) / b^2 - 1)^2 - 4*c^2/b^2 * cos(alpha)^2;
0029 A3 = 4*((a^2-c^2)/b^2*(1-(a^2-c^2)/b^2)*cos(beta) - ...
0030         (1-(a^2+c^2)/b^2)*cos(alpha)*cos(gamma) + ...
0031         2*c^2/b^2*cos(alpha)^2*cos(beta));
0032 A2 = 2*(((a^2-c^2)/b^2)^2 - 1 + 2*((a^2-c^2)/b^2)^2*cos(beta)^2 + ...
0033         2*(b^2-c^2)/b^2*cos(alpha)^2 - ...
0034         4*(a^2+c^2)/b^2*cos(alpha)*cos(beta)*cos(gamma) + ...
0035         2*(b^2-a^2)/b^2*cos(gamma)^2);
0036 A1 = 4*(-(a^2-c^2)/b^2*(1+(a^2-c^2)/b^2)*cos(beta) + ...
0037         2*a^2/b^2*cos(gamma)^2*cos(beta) - ...
0038         (1-(a^2+c^2)/b^2)*cos(alpha)*cos(gamma));
0039 A0 = (1+(a^2-c^2)/b^2)^2 - 4*a^2/b^2*cos(gamma)^2;
0040 % roots of polynomial
0041 v = roots ([A4, A3, A2, A1, A0]);
0042 solutions=0;
0043 % select real roots
0044 for i=1:length(v)
0045     if isreal(v(i)) && (v(i) > 0)
0046         solutions=solutions+1;
0047         s(solutions,1) = sqrt(b^2 / (1+v(i)^2-2*v(i)*cos(beta)));
0048         s(solutions,3) = v(i)*s(solutions,1);
0049         xxx = s(solutions,3)*cos(alpha);
0050         yyy = sqrt(s(solutions,3)^2*cos(alpha)^2+a^2-s(solutions,3)^2);
0051         s(solutions,2) = xxx + yyy;
0052         if (xxx > yyy)
0053             solutions = solutions+1;
0054             s(solutions,[1,3]) = s(solutions-1,[1,3]);
0055             s(solutions,2) = xxx - yyy;
0056         end;
0057     end;
0058 end;
0059 % initiate absolute orientation
0060 R = zeros(solutions, 3,3);
0061 Z = zeros(solutions, 3);
0062 b_o = X(3,:)' - X(1,:)';
0063 c_o = X(2,:)' - X(1,:)';
0064 R_o = [b_o / norm(b_o), cross(b_o,c_o)/norm(cross(b_o,c_o)), ...
0065         cross(b_o, cross(b_o, c_o)) / norm(cross(b_o, cross(b_o, c_o)))];
0066 % for each solution determine R and Z
0067 for i=1:solutions
0068     for j=1:3
0069         Xk(j,:) = s(i,j) * m(j,:);
0070     end;
0071     b_k = Xk(3,:)' - Xk(1,:)';
0072     c_k = Xk(2,:)' - Xk(1,:)';
0073     R_k = [b_k / norm(b_k), cross(b_k,c_k)/norm(cross(b_k,c_k)), ...
0074         cross(b_k, cross(b_k, c_k)) / norm(cross(b_k, cross(b_k, c_k)))];
0075     RR = R_k*R_o';
0076     R(i,:,:) = RR;
0077     Z(i,:) = (X(1,:)' - RR'*Xk(1,:)')';
0078 end;