190 lines
7.4 KiB
Matlab
190 lines
7.4 KiB
Matlab
close all
|
|
clear all
|
|
clc
|
|
|
|
filename = "rubiks.jpg"
|
|
|
|
% Prompt user to load data or select points manually
|
|
choice = menu('Do you want to load points from the workspace_variables.mat file?', 'Yes', 'No');
|
|
|
|
if choice == 1
|
|
% Load points from .mat file
|
|
load('workspace_variables.mat');
|
|
|
|
% Ensure the necessary variables exist in the loaded file
|
|
if exist('image_points', 'var') && exist('world_points', 'var')
|
|
disp('Points successfully loaded from workspace_variables.mat');
|
|
else
|
|
error('The loaded file does not contain the required variables "image_points" and "world_points".');
|
|
end
|
|
else
|
|
% Proceed with manual selection if user chooses "No"
|
|
|
|
% Load and display the image
|
|
img = imread(filename); % Replace with your image file path
|
|
imshow(img); % Display the image
|
|
hold on; % Keep the image displayed while adding markers
|
|
|
|
% Initialize matrices to store 2D and 3D homogeneous coordinates
|
|
num_points = 36; % Set number of points
|
|
image_points = zeros(num_points, 3); % For 2D image points in homogeneous coordinates
|
|
world_points = zeros(num_points, 4); % For 3D world points in homogeneous coordinates
|
|
|
|
disp('Click on the image to select 96 points and enter their 3D world coordinates.');
|
|
|
|
for i = 1:num_points
|
|
% Use ginput to get one image point at a time
|
|
[x, y] = ginput(1);
|
|
|
|
% Store the 2D image coordinates in homogeneous form
|
|
image_points(i, :) = [x, y, 1];
|
|
|
|
% Plot the selected point on the image
|
|
plot(x, y, 'r+', 'MarkerSize', 8, 'LineWidth', 1.5);
|
|
|
|
% Display the current point number for reference
|
|
text(x, y, sprintf(' %d', i), 'Color', 'yellow', 'FontSize', 10, 'FontWeight', 'bold');
|
|
|
|
% GUI prompt for 3D world coordinates
|
|
prompt = sprintf('Enter the 3D world coordinates for point %d (X, Y, Z):', i);
|
|
title = '3D World Coordinate Input';
|
|
dims = [1]; % Set single row dimension for the input dialog
|
|
default_input = {'0', '0', '0'}; % Default values for input fields
|
|
answer = inputdlg({'X:', 'Y:', 'Z:'}, title, dims, default_input);
|
|
|
|
% Convert input to numeric values and store in homogeneous form
|
|
if ~isempty(answer)
|
|
world_coords = str2double(answer); % Convert cell array of strings to numeric array
|
|
world_points(i, :) = [world_coords', 1]; % Store in homogeneous form [X Y Z 1]
|
|
else
|
|
error('3D coordinate input was canceled.'); % Handle case if input is canceled
|
|
end
|
|
end
|
|
|
|
% Release the hold on the image display
|
|
hold off;
|
|
end
|
|
|
|
% Construct the matrix A for solving P using SVD
|
|
A = [];
|
|
for i = 1:num_points
|
|
X = world_points(i, :); % Homogeneous 3D coordinates [X Y Z 1]
|
|
x = image_points(i, 1); % x-coordinate of the 2D image point
|
|
y = image_points(i, 2); % y-coordinate of the 2D image point
|
|
|
|
% Build the two rows for each point
|
|
row1 = [zeros(1, 4), -X, y * X];
|
|
row2 = [X, zeros(1, 4), -x * X];
|
|
|
|
% Append the rows to A
|
|
A = [A; row1; row2];
|
|
end
|
|
|
|
% Compute the camera projection matrix P using SVD
|
|
[~, ~, V] = svd(A);
|
|
P = reshape(V(:, end), 4, 3)'; % Reshape the last column of V into a 3x4 matrix
|
|
|
|
% Compute the camera center as the null space of P
|
|
[~, ~, V_P] = svd(P);
|
|
camera_center_h = V_P(:, end); % Last column of V
|
|
camera_center = camera_center_h(1:3) / camera_center_h(4); % Convert from homogeneous
|
|
|
|
% Decompose P into K and R using QR decomposition on the first 3x3 part
|
|
M = P(:, 1:3); % The 3x3 submatrix of P
|
|
[Q, R] = qr(inv(M)); % Use QR decomposition
|
|
K = inv(R); % Intrinsic matrix
|
|
K = K / K(3,3); % Normalize K so that K(3,3) is 1
|
|
R = inv(Q); % Rotation matrix
|
|
|
|
% Display computed matrices
|
|
disp('Computed camera matrix P:');
|
|
disp(P);
|
|
disp('Intrinsic matrix K:');
|
|
disp(K);
|
|
disp('Rotation matrix R:');
|
|
disp(R);
|
|
disp('Camera center (in world coordinates):');
|
|
disp(camera_center);
|
|
|
|
% Visualization in 3D
|
|
figure 1;
|
|
scatter3(world_points(:,1), world_points(:,2), world_points(:,3), 'bo'); % Plot 3D points
|
|
hold on;
|
|
scatter3(camera_center(1), camera_center(2), camera_center(3), 'ro', 'filled'); % Plot camera center
|
|
quiver3(camera_center(1), camera_center(2), camera_center(3), -R(3,1), -R(3,2), -R(3,3), 10, 'r'); % Plot principal axis
|
|
%title('3D Points, Camera Center, and Principal Axis');
|
|
xlabel('X');
|
|
ylabel('Y');
|
|
zlabel('Z');
|
|
legend('3D Points', 'Camera Center', 'Principal Axis');
|
|
grid on;
|
|
hold off;
|
|
|
|
% Back-project the 3D points onto the 2D image using the camera matrix P
|
|
projected_points = P * world_points';
|
|
|
|
% Convert from homogeneous coordinates to 2D by normalizing
|
|
projected_points = projected_points ./ projected_points(3, :);
|
|
|
|
% Display the original image and overlay the back-projected points
|
|
|
|
% Assuming P is already computed and available
|
|
|
|
% Define the points at infinity along the axes
|
|
D_x = [1; 0; 0; 0]; % Point at infinity along X-axis
|
|
D_y = [0; 1; 0; 0]; % Point at infinity along Y-axis
|
|
D_z = [0; 0; 1; 0]; % Point at infinity along Z-axis
|
|
D_o = [0; 0; 0; 1]; % World origin
|
|
|
|
% Project these points using the camera projection matrix P
|
|
image_point_infinity_x = P * D_x;
|
|
image_point_infinity_y = P * D_y;
|
|
image_point_infinity_z = P * D_z;
|
|
image_point_origin = P * D_o;
|
|
|
|
% Normalize the projected points to get 2D coordinates
|
|
image_point_infinity_x = image_point_infinity_x ./ image_point_infinity_x(3);
|
|
image_point_infinity_y = image_point_infinity_y ./ image_point_infinity_y(3);
|
|
image_point_infinity_z = image_point_infinity_z ./ image_point_infinity_z(3);
|
|
image_point_origin = image_point_origin ./ image_point_origin(3);
|
|
|
|
|
|
% Display the original image and overlay the points at infinity
|
|
figure(2);
|
|
imshow(img);
|
|
hold on;
|
|
|
|
% Plot the actual image points in red
|
|
scatter(image_points(:, 1), image_points(:, 2), 'ro', 'filled', 'DisplayName', 'Actual Image Points');
|
|
|
|
% Plot back-projected points in green
|
|
scatter(projected_points(1, :), projected_points(2, :), 'gx', 'filled', 'DisplayName', 'Back-Projected 3D Points');
|
|
|
|
% Plot points at infinity with different markers and colors
|
|
scatter(image_point_infinity_x(1), image_point_infinity_x(2), 'bx', 'LineWidth', 2, 'DisplayName', 'Point at Infinity (X-axis)');
|
|
scatter(image_point_infinity_y(1), image_point_infinity_y(2), 'gx', 'LineWidth', 2, 'DisplayName', 'Point at Infinity (Y-axis)');
|
|
scatter(image_point_infinity_z(1), image_point_infinity_z(2), 'mx', 'LineWidth', 2, 'DisplayName', 'Point at Infinity (Z-axis)');
|
|
scatter(image_point_origin(1), image_point_origin(2), 'ko', 'filled', 'DisplayName', 'World Origin');
|
|
|
|
% Draw vanishing lines from world origin to points at infinity
|
|
plot([image_point_origin(1), image_point_infinity_x(1)], ...
|
|
[image_point_origin(2), image_point_infinity_x(2)], ...
|
|
'b--', 'LineWidth', 1.5, 'DisplayName', 'Vanishing Line (X-axis)');
|
|
|
|
plot([image_point_origin(1), image_point_infinity_y(1)], ...
|
|
[image_point_origin(2), image_point_infinity_y(2)], ...
|
|
'g--', 'LineWidth', 1.5, 'DisplayName', 'Vanishing Line (Y-axis)');
|
|
|
|
% Draw a line connecting points at infinity in X and Y directions
|
|
plot([image_point_infinity_x(1), image_point_infinity_y(1)], ...
|
|
[image_point_infinity_x(2), image_point_infinity_y(2)], ...
|
|
'm--', 'LineWidth', 1.5, 'DisplayName', 'Line Between Infinity Points');
|
|
|
|
% Add title
|
|
|
|
% Create legend
|
|
%legend show; % Show legend to display all categories
|
|
|
|
hold off;
|
|
|