Affine Structure From Motion

Implementation of the affine structure from motion procedure described in Shape and Motion from Image Streams under Orthography: a Factorization Method” 1992 by Tomasi and Kanade.
The goal of this project is to reconstruct the 3D shape of an object from the tracked keypoints. I will not implement feature tracking. Therefore, if you want to learn about keypoint selection and feature tracking, please look into Harris corner detection and Kanede-Lucus-Tomasi feature tracking respectively.

Matlab Code
% Load tracked_points.mat file
folder = '...';
fullMatFileName = fullfile(folder, 'tracked_points.mat');
if ~exist(fullMatFileName, 'file')
  message = sprintf('%s does not exist', fullMatFileName);
  tracked_points = load(fullMatFileName);

Xs = tracked_points.Xs;
Ys = tracked_points.Ys;

[xSize1 xSize2] = size(Xs);

% Create zero matrix with the size of combination of tracked_points
D = zeros((xSize1*2), xSize2);
D(1:2:end, :) = Xs;
D(2:2:end, :) = Ys;

% Singular value decomposition of matrix D
[U W V] = svd(D);

% First three(3) columns of matrix U
U = U(:, 1:3);

% First three(3) columns and rows of matrix W. (3x3 matrix)
W = W(1:3, 1:3);

% First three(3) rows of transpose of matrix V
V = V'; % Transpose
V = V(1:3, :);

% Create the motion and shape matrices:
M = U * sqrtm(W);
S = sqrtm(W) * V;

Q =[];
R = [];

for i = 1 : xSize1

    G = M(2*i-1, :);
    a = G(1); b = G(2); c = G(3);
    G = M(2*i, :);
    d = G(1); e = G(2); f = G(3);
    Q = [Q;
         a*a a*b+b*a a*c+c*a b*b c*b+b*c c*c;
         d*d d*e+e*d d*f+f*d e*e f*e+e*f f*f;
         a*d a*e+b*d a*f+c*d b*e b*f+e*c c*f];
    R = [R; 1; 1; 0];

l = Q \ R;

L = [l(1) l(2) l(3);
     l(2) l(4) l(5);
     l(3) l(5) l(6)];
T = chol(L);

M = M * T;
S = inv(T) * S;

% Plot the predicted 3D locations of the tracked points for 3 different viewpoints. Choose the viewpoints so that the 3D structure is clearly visible.
rotate3d on; %grid on;
axis('equal'); axis vis3d; axis tight;
view(50, 20);
grid on;
hold on;
plot3(S(1, :), S(2, :), S(3, :), 'k.');
%scatter3(S(1,:), S(2,:), S(3,:), ones(1, size(S, 2)) * 100, 'g');
suptitle('Predicted 3D locations of the tracked points');

%% 3D keypoints from different viewpoints
figure; axis('equal'); plot3(S(1, :), S(3, :), S(2, :), 'k.'); xlabel('x coordinate'); ylabel('z coordinate'); zlabel('y coordinate');
figure; axis('equal'); plot3(S(2, :), S(3, :), S(1, :), 'k.'); xlabel('y coordinate'); ylabel('z coordinate'); zlabel('x coordinate');
figure; axis('equal'); plot3(S(3, :), S(2, :), S(1, :), 'k.'); xlabel('z coordinate'); ylabel('y coordinate'); zlabel('x coordinate');

% Plot the predicted 3D path of the cameras. The camera position for each frame is given by the cross product k = i x j.
% For consistent results, normalize all k to be the same length (i.e. unit vectors).
camera_positions = zeros(xSize1, 3);
for f = 1 : xSize1
    kf = cross(M(2*f-1, :), M(2*f, :));
    camera_positions(f, :) = kf / norm(kf); % in unit norm

cposX = camera_positions(:, 1);
cposY = camera_positions(:, 2);
cposZ = camera_positions(:, 3);

rotate3d on; %grid on;
axis('equal'); axis vis3d; axis tight;
plot3(cposX, cposY, cposZ,'.-');
xlabel('X Coordinate'); ylabel('Z Coordinate'); zlabel('Y Coordinate');
suptitle('The camera position for each frame - 3D View');
grid on;

subplot(131);plot(cposX, cposY, 'g.-');
grid on;    title('XY axis');
subplot(132);plot(cposX, cposZ, 'g.-');
grid on;    title('XZ axis');
subplot(133); plot(cposY, cposZ, 'g.-');
grid on;  title('YZ axis');
suptitle('Predicted 3D path of the cameras');

If you liked the content please share it!

You may also like...

Leave a Reply

Your email address will not be published.

This site uses Akismet to reduce spam. Learn how your comment data is processed.