Colorado School of Mines. Computer Vision. Professor William Hoff Dept of Electrical Engineering &Computer Science.

Transcription

1 Proessor William Ho Dept o Electrical Engineering &Computer Science

2 Pose Estimation

3 Model based Pose Estimation Problem Statement Given We have an image o an object We know the model geometry o the object (speciically, the location o eatures on the object) We have ound the corresponding eatures in the image Find The position and orientation (pose) o the object with respect to the camera # points needed? Assumptions Object is rigid (so 6 only degrees o reedom) Camera intrinsic parameters are known We will ind the pose that minimizes the squared error o the predicted locations o the image eatures, to the measured locations {M} C we want M H 3

4 Least Squares Pose Estimation Let y = () is a vector o the unknown pose parameters y y is a unction that returns the predicted image z points y, given the pose y y, t y0 is a vector o the actual observed image points t y We want to ind to minimize E = () y0 tz Algorithm:. We start with a guess or, call it 0. Compute y = (). Residual error is dy = y y0 3. Calculate Jacobian o, J = [ / ], and evaluate it at. We now have dy = Jd 4. Solve or d using pseudo inverse d = (J T J) J T dy 5. Set <= + d 6. Repeat steps 5 until convergence (no more change in ) y 4

5 Recall Perspective Projection Projection o a 3D point W P in the world to a point in the piel image ( im,y im ) Where the etrinsic parameter matri is Or, i we use model instead o world rame or the point: /, /, y Z Y X im im W et K M X C C et W Worg Y Z r r r t r r r t r r r t M R t y y c c K And the intrinsic parameter matri ~ P K M p W et P R K P K M p M Morg C C M M et t ~ 5

6 Recall XYZ ied angle convention R = Rz Ry R, where RZ cosz sinz 0 sin cos 0 Z Z 0 0 R Y cosy 0 siny 0 0 siny 0 cos Y R X cos sin X X 0 sin X cos X Matlab R = [ 0 0; 0 cos(a) -sin(a); 0 sin(a) cos(a)]; Ry = [ cos(ay) 0 sin(ay); 0 0; -sin(ay) 0 cos(ay)]; Rz = [ cos(az) -sin(az) 0; sin(az) cos(az) 0; 0 0 ]; R = Rz * Ry * R ote in general, the angle ais convention would be better to use than XYZ angles 6

7 Function to project one point Write a unction to project a 3D point P_M in model coordinates to image point p, given the model to camera pose = (a,ay,az,t,ty,tz) P_M = [X;Y;Z;] is the input point = [a;ay;az;t;ty;tz] is the vector o model to camera pose parameters K = intrinsic camera matri p = [;y] is the output point unction p = Project(, P_M, K) % Project 3D point onto image % Get pose params a = (); ay = (); az = (3); t = (4); ty = (5); tz = (6); % Rotation matri, model to camera R = [ 0 0; 0 cos(a) -sin(a); 0 sin(a) cos(a)]; Ry = [ cos(ay) 0 sin(ay); 0 0; -sin(ay) 0 cos(ay)]; Rz = [ cos(az) -sin(az) 0; sin(az) cos(az) 0; 0 0 ]; R = Rz * Ry * R; % Etrinsic camera matri Met = [ R [t;ty;tz] ]; % Project point ph = K*Met*P_M; ph = ph/ph(3); p = ph(:); return 7

8 Function to transorm a set o points ow modiy the unction to transorm a set o points P_M = is a set o input points = [a;ay;az;t;ty;tz] is the pose K = intrinsic camera matri p = [;y;;y; ] are the output points unction p = Project(, P_M, K) 8 M Z Z Z Y Y Y X X X P y y y p

9 Function to transorm a set o points unction p = Project(, P_M, K) % Project 3D points onto image % Get pose params a = (); ay = (); az = (3); t = (4); ty = (5); tz = (6); % Rotation matri, model to camera R = [ 0 0; 0 cos(a) -sin(a); 0 sin(a) cos(a)]; Ry = [ cos(ay) 0 sin(ay); 0 0; -sin(ay) 0 cos(ay)]; Rz = [ cos(az) -sin(az) 0; sin(az) cos(az) 0; 0 0 ]; R = Rz * Ry * R; % Etrinsic camera matri Met = [ R [t;ty;tz] ]; % Project points ph = K*Met*P_M; % Divide through 3rd element o each column ph(,:) = ph(,:)./ph(3,:); ph(,:) = ph(,:)./ph(3,:); ph = ph(:,:); % Get rid o 3rd row p = reshape(ph, [], ); % reshape into vector return 9

10 Eample Focal length in piels: 75 Image center (,y): (354, 45) 0

11 clear all close all I = imread('img_rect.ti'); imshow(i, []) Eample % These are the points in the model's coordinate system (inches) P_M = [ ; ; ; ]; % Deine camera parameters = 75; % ocal length in piels c = 354; cy = 45; K = [ 0 c; 0 cy; 0 0 ]; % intrinsic parameter matri y0 = [ 83; 47; % 350; 33; % 454; 44; % 3 76; 58; % 4 339; 75; % 5 444; 86 ]; % 6 % Make an initial guess o the pose [a ay az t ty tz] = [.5; -.0; 0.0; 0; 0; 30]; % Get predicted image points by substituting in the current pose y = Project(, P_M, K); or i=::length(y) rectangle('position', [y(i)-8 y(i+)-8 6 6], 'FaceColor', 'r'); end

12 Computing Jacobian umerically We approimate the derivatives Matlab code: ) ( ) ˆ ( ) ( u i i M M M j i J y = Project(,P_M,K); e = ; % a tiny number J(:,) = ( Project(+[e;0;0;0;0;0],P_M,K) y )/e; J(:,) = ( Project(+[0;e;0;0;0;0],P_M,K) y )/e; J(:,3) = ( Project(+[0;0;e;0;0;0],P_M,K) y )/e; J(:,4) = ( Project(+[0;0;0;e;0;0],P_M,K) y )/e; J(:,5) = ( Project(+[0;0;0;0;e;0],P_M,K) y )/e; J(:,6) = ( Project(+[0;0;0;0;0;e],P_M,K) y )/e;

13 We have y0 = observations or measurements 0 = a guess or y = () is a non linear unction. Initialize to 0. Compute y = (). Residual error is dy = y y0 3. Calculate Jacobian o, evaluate it at. We now have dy = Jd 4. Solve or d using pseudo inverse d = (J T J) J T dy 5. Set <= + d 6. Repeat steps 5 until convergence (no more change in ) or i=:0 print('\niteration %d\ncurrent pose:\n', i); disp(); % Get predicted image points y = Project(, P_M, K); imshow(i, []) or i=::length(y) rectangle('position', [y(i)-8 y(i+)-8 6 6],... 'FaceColor', 'r'); end pause(); % Estimate Jacobian e = ; % a tiny number : % Error is observed image points - predicted image points dy = y0 - y; print('residual error: %\n', norm(dy)); % Ok, now we have a system o linear equations dy = J d % Solve or d using the pseudo inverse d = pinv(j) * dy; % Stop i parameters are no longer changing i abs( norm(d)/norm() ) < e-6 break; end end = + d; % Update pose estimate 3

14 or i=:0 print('\niteration %d\ncurrent pose:\n', i); disp(); % Get predicted image points y = Project(, P_M, K); imshow(i, []) or i=::length(y) rectangle('position', [y(i)-8 y(i+)-8 6 6],... 'FaceColor', 'r'); end pause(); % Estimate Jacobian e = ; % a tiny number J(:,) = ( Project(+[e;0;0;0;0;0],P_M,K) - y )/e; J(:,) = ( Project(+[0;e;0;0;0;0],P_M,K) - y )/e; J(:,3) = ( Project(+[0;0;e;0;0;0],P_M,K) - y )/e; J(:,4) = ( Project(+[0;0;0;e;0;0],P_M,K) - y )/e; J(:,5) = ( Project(+[0;0;0;0;e;0],P_M,K) - y )/e; J(:,6) = ( Project(+[0;0;0;0;0;e],P_M,K) - y )/e; % Error is observed image points - predicted image points dy = y0 - y; print('residual error: %\n', norm(dy)); % Ok, now we have a system o linear equations dy = J d % Solve or d using the pseudo inverse d = pinv(j) * dy; % Stop i parameters are no longer changing i abs( norm(d)/norm() ) < e-6 break; end = + d; % Update pose estimate end 4

15 Overlaying Graphical Model As a check, overlay a graphical model onto the image I you don t have a model, you can display the model s coordinate aes a=.55 ay=-0.83 az=0.0 t=0.9 ty=3.0 tz=8.3 % Draw coordinate aes onto the image. Scale the length o the aes % according to the size o the model, so that the aes are visible. W = ma(p_m,[],) - min(p_m,[],); % Size o model in X,Y,Z W = norm(w); % Length o the diagonal o the bounding bo u0 = Project(, [0;0;0;], K); % origin ux = Project(, [W/5;0;0;], K); % unit X vector uy = Project(, [0;W/5;0;], K); % unit Y vector uz = Project(, [0;0;W/5;], K); % unit Z vector line([u0() ux()], [u0() ux()], 'Color', 'r', 'LineWidth', 3); line([u0() uy()], [u0() uy()], 'Color', 'g', 'LineWidth', 3); line([u0() uz()], [u0() uz()], 'Color', 'b', 'LineWidth', 3); % Also print the pose onto the image. tet(30,450,sprint('a=%. ay=%. az=%. t=%. ty=%. tz=%.',... (), (), (3), (4), (5), (6)),... 'BackgroundColor', 'w', 'FontSize', 5); 5

16 Other images Try inding the pose o the bo in the other two images How close (approimately) does the initial guess have to be? img_rect.ti img3_rect.ti 6