I'm currently trying to compute the camera matrix P given a set of world points (X) with its corresponding image points (x). However, when testing for the the result, P (3 x 4 camera matrix) multiplying by the world points does not give me the correct corresponding image points. However, only the first column of PX = x. The other column won't return the approximate image points.
Code:
X = [1 2 3; 4 5 6; 7 8 9; 1 1 1];
x = [3 2 1; 6 5 4; 1 1 1];
[mX, nX] = size(X);
[mx, nx] = size(x);
for i = 0:(nX-1)
XX{i+1} = transpose(X(1+i: 4+i));
end
for i = 0:(nx-1)
xx{i+1} = transpose(x(i+1:3+i));
end
%TODO - normalization
A = [];
%construct matrix
for i = 1:nX
A = [A; zeros(1,4) -1*(xx{i}(3)*transpose(XX{i})) xx{i}(2)*transpose(XX{i})];
A = [A; xx{i}(3)*transpose(XX{i}) zeros(1,4) -1*xx{i}(1)*transpose(XX{i})];
end
%using svd to solve for non zero solution
[u s v] = svd(A);
p = v(:, size(v,2));
p = reshape(p, 4,3)';
output for the first column, works good:
>> p*XX{1}
ans =
0.0461
0.0922
0.0154
>> ans/0.0154
ans =
2.9921
5.9841
0.9974
>> xx{1}
ans =
3
6
1
output for the second column, doesn't work:
>> p*XX{2}
ans =
0.5202
0.0867
0.1734
>> ans/0.1734
ans =
2.9999
0.5000
1.0000
>> xx{2}
ans =
6
1
2
By the way, I was told that I need to normalize the world points and image points before I compute the camera matrix. I have not done this step and have no idea how to. If this is causing the issue, please explain what can be done. Thank you in advance.
This is because you aren't indexing into the matrix properly. You are using linear indexing to access each column of the matrix. In that case, your for loop needs to access each column independently. Therefore, each iteration of your for loop must access groups of 4 elements for your 3D points and groups of 3 elements for your 2D points.
As such, you simply need to do this for your for loops:
for i = 0:(nX-1)
XX{i+1} = transpose(X(4*i + 1 : 4*(i + 1)));
end
for i = 0:(nx-1)
xx{i+1} = transpose(x(3*i + 1 : 3*(i + 1)));
end
After this, the code should work no problem. To verify, we can loop through each 3D point and determine its 2D equivalent as you're using cells:
out = zeros(size(xx)); % Declare output matrix
for ii = 1 : numel(XX) % For each 3D point...
out(:,ii) = p * XX{ii}; % Transform the point
out(:,ii) = out(:,ii) / out(end,ii); % Normalize
end
We thus get:
>> out
out =
3.0000 2.0000 1.0000
6.0000 5.0000 4.0000
1.0000 1.0000 1.0000
Compare with your x:
>> x
x =
3 2 1
6 5 4
1 1 1
If I can suggest something, please do not use cell arrays here. You can create the matrix of equations for solving using vectorization. Specifically, you can create the matrix A directly without any for loops:
A = [zeros(N, 4) -X.' bsxfun(@times, x(2,:).', X.');
X.' zeros(N, 4) bsxfun(@times, -x(1,:).', X.')];
If you own MATLAB R2016b and up, you can do this with internal broadcasting:
A = [zeros(N, 4) -X.' x(2,:).' .* X.';
X.' zeros(N, 4) -x(1,:).' .* X.']
Note that you will see the rows are shuffled in comparison to your original matrix A because of the vectorization. Because we are solving for the null space of the matrix A, shuffling the rows has no effect. Therefore, your code can be simplified to:
X = [1 2 3; 4 5 6; 7 8 9; 1 1 1];
x = [3 2 1; 6 5 4; 1 1 1];
A = [zeros(N, 4) -X.' bsxfun(@times, x(2,:).', X.');
X.' zeros(N, 4) bsxfun(@times, -x(1,:).', X.')];
% Use this for MATLAB R2016b and up
% A = [zeros(N, 4) -X.' x(2,:).' .* X.';
% X.' zeros(N, 4) -x(1,:).' .* X.']
[u, s, v] = svd(A);
p = v(:, end);
p = reshape(p, 4, 3).';
To finally compute the output matrix, you can just use simple matrix multiplication. The fact that you are using cells requires that you have to use a for loop and it's much faster to do this with matrix multiplication:
out = p * X;
You can then take the last row of the results and divide each of the other rows by this row.
out = bsxfun(@rdivide, out, out(end,:));
Again with MATLAB R2016b and up, you can just do it as so:
out = out ./ out(end,:);