function [R,T] = cv_3d_pose(image_pts, model_pts,R0,T0)
%
[num_pts,dummy] = size(model_pts);
ip2 = image_pts';
Pm = model_pts';
R = eye(3);
T = zeros(3,1);
R = R0;
T = T0;
residual_mag = 1000000; old_mag = inf;
thresh = 0.01;

while (residual_mag>thresh) & (old_mag>residual_mag)
   P3 = R*Pm;
   for p = 1:num_pts
      P3(:,p) = P3(:,p) + T;
   end
   for p = 1:num_pts
      P2(1,p) = P3(1,p)/P3(3,p);
      P2(2,p) = P3(2,p)/P3(3,p);
   end
   r_index = 1;
   for p = 1:num_pts
      b(r_index,1) = P2(1,p) - ip2(1,p);
      b(r_index+1,1) = P2(2,p) - ip2(2,p);
      r_index = r_index+2;
   end
   r_index = 1;
   for p = 1:num_pts
      A(r_index,1) = 1/P3(3,p);
      A(r_index,2) = 0;
      A(r_index,3) = -P3(1,p)/(P3(3,p)^2);
      A(r_index,4) = -(P3(1,p)*P3(3,p)^2);
      A(r_index,5) = (P3(1,p)^2 + P3(3,p)^2)/(P3(3,p)^2);
      A(r_index,6) = -P3(2,p)/P3(3,p);
      A(r_index+1,1) = 0;
      A(r_index+1,2) = 1/P3(3,p);
      A(r_index+1,3) = -P3(2,p)/(P3(3,p)^2);
      A(r_index+1,4) = -(P3(3,p)^2 + P3(2,p)^2)/(P3(3,p)^2);
      A(r_index+1,5) = P3(1,p)*P3(2,p)/(P3(3,p)^2);
      A(r_index+1,6) = P3(1,p)/P3(3,p);
      r_index = r_index+2;
   end
   x = A\b;
   T = T + [x(1) x(2) x(3)]';
   R = R*cv_angles2R(x(4),x(5),x(6));
   old_mag = residual_mag;
   residual_mag = norm(b,2);
end

