diff --git a/CalibratedToUncalibrated.m b/CalibratedToUncalibrated.m new file mode 100644 index 0000000..d927295 --- /dev/null +++ b/CalibratedToUncalibrated.m @@ -0,0 +1,10 @@ +function P_un = CalibratedToUncalibrated(P_ca, K) + +N = length(P_ca); + +P_un = cell(1,N); +for i=1:N + P_un{i} = K * P_ca{i}; +end + +end \ No newline at end of file diff --git a/Cheirality_triangulate.m b/Cheirality_triangulate.m new file mode 100644 index 0000000..5f31ec6 --- /dev/null +++ b/Cheirality_triangulate.m @@ -0,0 +1,85 @@ +function [X_best, P] = Cheirality_triangulate(x1_h_n_in, x2_h_n_in, E_ransac) +% CHEIRALITY_TRIANGULATE - Triangulate 3D points and resolve E ambiguity +% Inputs: +% x1, x2 - 2D unnormalized homogeneous points in images 1 and 2 (3xN) +% inliers - Indices of inlier matches after RANSAC +% K - Intrinsic camera matrix (3x3) +% E_ransac - Estimated essential matrix (3x3) +% Outputs: +% X_best - Triangulated 3D points (4xN homogeneous coordinates) +% P - Correct camera projection matrices {P1, P2} + + % Use the selected inlier points for triangulation + + % Initialize best parameters + P2_correct_ind = 0; % Index for correct P2 + max_points_1 = 0; % Maximum valid points satisfying chirality + X_best = []; % Store the best 3D points + + % Step 2: Define camera matrices + % First camera at the origin (canonical form) + P1 = [eye(3), zeros(3, 1)]; + + % Step 3: Decompose Essential Matrix (E) to get R, t candidates + [U, ~, V] = svd(E_ransac); + W = [0 -1 0; 1 0 0; 0 0 1]; % Pre-defined rotation matrix + + % Four possible solutions for [R, t] + R1 = U * W * V'; + R2 = U * W.'*V'; + + t = U(:, 3); + + % Ensure proper rotations (determinant = +1) + if det(R1) < 0, R1 = -R1; end + if det(R2) < 0, R2 = -R2; end + + % Step 4: Generate P2 candidates (Four cases) + P2_candidates = { + [R1, t], + [R1, -t], + [R2, t], + [R2, -t] + }; + + % Step 5: Evaluate each P2 candidate based on chirality condition + for i = 1:4 + P2 = P2_candidates{i}; + + % Triangulate 3D points using current P2 + X = triangulate_3D_point_DLT(x1_h_n_in, x2_h_n_in, P1, P2); + + % Project back to both views + x1_proj = P1 * X; + x2_proj = P2 * X; + + % Count points in front of both cameras (chirality check) + valid_points = (x1_proj(3,:) > 0) & (x2_proj(3,:) > 0); + num_valid = sum(valid_points); + + + % Keep the candidate with the most valid points + if num_valid > max_points_1 + max_points_1 = num_valid; + P2_correct_ind = i; + X_best = X; % (:, valid_points); % Store the best 3D points % Filter valid 3D points + end + end + + + % Step 6: Final output - Correct projection matrices + P{1} = P1; + P{2} = P2_candidates{P2_correct_ind}; + + info('\n--- Cheirality_triangulate Validation Start ---\n',2); + + % Print the selected solution + info('Selected P2 Index: %d\n',2,P2_correct_ind); + info('Number of Valid Points: %d out of %d\n',2, max_points_1, length(x1_h_n_in)); + + % Optional: Compute reprojection error + err1 = compute_reprojection_error_P(P{1}, X_best, x1_h_n_in); % (:, valid_points)); + err2 = compute_reprojection_error_P(P{2}, X_best, x2_h_n_in); % (:, valid_points)); + info('Mean Reprojection Error: %.4f (Image 1), %.4f (Image 2)\n',2, mean(err1), mean(err2)); + info('\n--- Cheirality_triangulate Validation End ---\n',2); +end \ No newline at end of file diff --git a/Helper functions/Cheirality_triangulate.m b/Helper functions/Cheirality_triangulate.m deleted file mode 100644 index f468f93..0000000 --- a/Helper functions/Cheirality_triangulate.m +++ /dev/null @@ -1,59 +0,0 @@ -function [X_best,P] = Cheirality_triangulate(x1,x2, inliers,K,E_ransac) - - % Include only the inliers on the triangulation - x1_inliers = x1(:,inliers); - x2_inliers = x2(:,inliers); - - % Correct camera P2 - P2_correct_ind = 0 ; - max_points_1 = 0; - - % X best - X_best = []; - - % Define the cameras & assume first camera is at the origin - P1 = K* [eye(3), zeros(3, 1)]; - - % Get the U and V canditates - [U, ~, V] = svd(E_ransac); - W = [0 -1 0; 1 0 0; 0 0 1]; % Rotation matrix for decomposition - - % Four possible solutions for P2 - R1 = U * W * V'; - R2 = U * W' * V'; - t = U(:, 3); - - % Ensure proper rotations (determinant = +1) - if det(R1) < 0, R1 = -R1; end - if det(R2) < 0, R2 = -R2; end - - % Four possible camera matrices - P2_candidates = { - K * [R1, t], - K * [R1, -t], - K * [R2, t], - K * [R2, -t] - }; - - % Choose the correct P2 by testing chirality (points must be in front of both cameras) - for i = 1:4 - P2 = P2_candidates{i}; - - X = triangulate_3D_point_DLT(x1_inliers, x2_inliers,P1, P2); - x1_proj = P1 * X; - x2_proj = P2 * X; - - % This is a general condition for the position of the points - cond = sum( (x1_proj(3,:) > 0) & ((x2_proj(3,:) > 0)) ); - - if cond > max_points_1 - max_points_1 = cond; - P2_correct_ind = i; - X_best = X; - end - end - - P{1} = P1; - P{2} = P2_candidates{P2_correct_ind}; -end - diff --git a/Helper functions/enforce_essential.m b/Helper functions/enforce_essential.m deleted file mode 100644 index e0f5ec3..0000000 --- a/Helper functions/enforce_essential.m +++ /dev/null @@ -1,15 +0,0 @@ -function E = enforce_essential(E_approx) -% ENFORCE_ESSENTIAL Apply the enforce on E - -% Apply the SVD on E approximate -[U,~,V] = svd( E_approx ); - -if det(U*V') < 0 - V = -V; -end - -% Creates a valid essential matrix from an approximate solution - -E = U * diag ([1 1 0])* V'; - -end \ No newline at end of file diff --git a/Helper functions/estimate_T_robust.m b/Helper functions/estimate_T_robust.m deleted file mode 100644 index 070f831..0000000 --- a/Helper functions/estimate_T_robust.m +++ /dev/null @@ -1,107 +0,0 @@ -% Function: Robust Translation Estimation -function T_best = estimate_T_robust(xs, Xs, R, inlier_threshold, T_init) - - % Initialization - max_inliers = 0; - N = size(xs, 2); - iterations = 10000; % RANSAC iterations - - for i = 1:iterations - % Sample minimal points (2 points for translation) - sample_indices = randperm(N, 2); - xs_sample = xs(:, sample_indices); - Xs_sample = Xs(:, sample_indices); - - % Estimate candidate T using DLT - T_candidate = estimate_translation_DLT(xs_sample, Xs_sample, R); - - % Compute reprojection errors - errors = compute_reprojection_errors(xs, Xs, R, T_candidate); - - % Count inliers - inliers_candidate = errors.^2 < inlier_threshold.^2; - num_inliers = sum(inliers_candidate); - - % Update best model - if num_inliers > max_inliers - max_inliers = num_inliers; - T_best = T_candidate; - inliers = inliers_candidate; - end - end - - % Refine with inliers - if max_inliers > 0 - T_best = estimate_translation_DLT(xs(:, inliers), Xs(:, inliers), R); - else - warning('No inliers found. Returning initial translation.'); - T_best = T_init; - end -end - - -% Function: Translation Estimation using DLT -function T = estimate_translation_DLT(x, X, R) - % Inputs: - % x - 2D normalized points in homogeneous coordinates (3xN) - % X - 3D points in homogeneous coordinates (4xN) - % R - Rotation matrix (3x3) - - % Outputs: - % T - Estimated translation vector (3x1) - - % Number of points - N = size(x, 2); - - % Initialize matrices A and b - A = []; - b = []; - - % Loop through each point - for i = 1:N - % Extract the 2D point (x) and 3D point (X) - xi = x(:, i); % 2D point in homogeneous coordinates - Xi = X(:, i); % 3D point in homogeneous coordinates - - % Create the linear system based on the equations in the image - A_i = [ - Xi(4), 0, 0, -xi(1); % First row - 0, Xi(4), 0, -xi(2); % Second row - 0, 0, Xi(4), -xi(3) % Third row - ]; - - % Append to the full matrix A - A = [A; A_i]; - - % Compute b using the rotation matrix R - b_i = -R * Xi(1:3); % Projected 3D point - b = [b; b_i]; - end - - % Solve the linear system A * T = b using least squares - T_lambda = A \ b; - - % Extract translation vector T (first 3 elements) - T = T_lambda(1:3); - - % Compute residual error - % residual = norm(A * T_lambda - b); % ||A * T_lambda - b|| - % disp('Residual Error:'); - % disp(residual); - - % Flip sign if necessary based on direction consistency - if dot(T, mean(X(1:3, :), 2)) < 0 - T = -T; % Flip the translation vector - end -end - - -% Function: Compute Reprojection Errors -function errors = compute_reprojection_errors(xs, Xs, R, T) - % Project 3D points - projected = (R * Xs(1:3, :) + T); - projected = projected(1:2, :) ./ projected(3, :); % Normalize - - % Compute errors - errors = sqrt(sum((xs(1:2, :) - projected).^2, 1)); % Euclidean distance -end diff --git a/Helper functions/feature_extraction.m b/Helper functions/feature_extraction.m deleted file mode 100644 index b7c6b35..0000000 --- a/Helper functions/feature_extraction.m +++ /dev/null @@ -1,30 +0,0 @@ -function [x1,x2, desc_X, fA, dA, fB, dB] = feature_extraction(im1,im2) - - % Return a homogenous projections - - % Extract the features using SIFT - % 1 specifies a threshold for rejecting low-contrast keypoints detected by SIFT. - - [fA dA] = vl_sift(single(rgb2gray(im1)), 'PeakThresh', 1); - [fB dB] = vl_sift(single(rgb2gray(im2)), 'PeakThresh', 1); - - % Compute the matches - matches = vl_ubcmatch(dA, dB); - - % Get the 2D points correspondences on each images' pair - xA = fA(1:2 , matches(1 ,:)); - xB = fB(1:2 , matches(2 ,:)); - - % Convert the vectors to be homogenous for this pair of images - % Add a homogenous dimension to the points - x1 = [xA; ones(1,length(xA))]; - x2 = [xB; ones(1,length(xB))]; - - % Save descriptors for matched points (3D points will correspond to these) - % Extract descriptors only for matched points in image 1 - desc_X = dA(:, matches(1, :)); - -end - -%% - diff --git a/Helper functions/optimize_translations.m b/Helper functions/optimize_translations.m deleted file mode 100644 index 5fc5f93..0000000 --- a/Helper functions/optimize_translations.m +++ /dev/null @@ -1,115 +0,0 @@ -function [T1_refined, T2_refined, accumulated_error] = optimize_translations(K, R1, R2, T1, T2, X, x1, x2, mu, max_iterations, tolerance) - % OPTIMIZE_TRANSLATIONS - Refines translation vectors (T1, T2) using - % Levenberg-Marquardt optimization. - - % Initialize variables - T1_temp = T1; % Translation for camera 1 - T2_temp = T2; % Translation for camera 2 - accumulated_error = zeros(1, max_iterations); % Error log for plotting - - % Optimization loop - for iter = 1:max_iterations - % Compute Total Error Before Update - total_error_before = 0; - for j = 1:size(X, 2) - err = ComputeReprojectionError(K, R1, R2, T1_temp, T2_temp, X(:, j), x1(:, j), x2(:, j)); - total_error_before = total_error_before + err; - end - accumulated_error(iter) = total_error_before; % Store error - - % Compute Jacobian and Residual - [r, J] = ComputeJacobianAndResidual(K, R1, R2, T1_temp, T2_temp, X, x1, x2); - - % Compute LM Update - delta_T = ComputeUpdate(r, J, mu); % Update for T1 and T2 - - % Apply Updates - T1_test = T1_temp + delta_T(1:3); % Update T1 - T2_test = T2_temp + delta_T(4:6); % Update T2 - - % Compute Total Error After Update - total_error_after = 0; - for j = 1:size(X, 2) - err = ComputeReprojectionError(K, R1, R2, T1_test, T2_test, X(:, j), x1(:, j), x2(:, j)); - total_error_after = total_error_after + err; - end - - % Accept or Reject Update - if total_error_after < total_error_before - T1_temp = T1_test; - T2_temp = T2_test; - mu = mu / 2; % Decrease damping - else - mu = mu * 2; % Increase damping - end - - % Check Convergence - if abs(total_error_after - total_error_before) < tolerance - break; % Converged - end - end - - % Return refined translations - T1_refined = T1_temp; - T2_refined = T2_temp; -end -%% ---------------------------------------------------------- -% Helper Functions -%% ---------------------------------------------------------- - -% Compute Reprojection Error -function error = ComputeReprojectionError(K, R1, R2, T1, T2, X, x1, x2) - % Projection matrices - P1 = K * [R1, T1]; % Camera 1 - P2 = K * [R2, T2]; % Camera 2 - - % Ensure homogeneous coordinates - if size(X, 1) == 3 - X = [X; 1]; - end - - % Project points - x1_proj = P1 * X; - x2_proj = P2 * X; - - % Normalize projections - x1_proj = x1_proj(1:2) / x1_proj(3); - x2_proj = x2_proj(1:2) / x2_proj(3); - - % Compute error - err1 = norm(x1_proj - x1(1:2)); - err2 = norm(x2_proj - x2(1:2)); - error = err1 + err2; -end - -% Compute Jacobian and Residuals -function [r, J] = ComputeJacobianAndResidual(K, R1, R2, T1, T2, X, x1, x2) - % Ensure homogeneous coordinates - if size(X, 1) == 3 - X = [X; 1]; - end - - % Compute projections - P1 = K * [R1, T1]; - P2 = K * [R2, T2]; - x1_proj = P1 * X; - x2_proj = P2 * X; - x1_proj = x1_proj(1:2) / x1_proj(3); - x2_proj = x2_proj(1:2) / x2_proj(3); - - % Compute residuals - r = [x1_proj - x1(1:2); x2_proj - x2(1:2)]; - - % Compute Jacobian for T1 and T2 - J1 = K * R1; % Partial derivatives w.r.t T1 - J2 = K * R2; % Partial derivatives w.r.t T2 - - % Final Jacobian (4x6) - J = [J1;J2]; -end - -function delta = ComputeUpdate(r, J, mu) - % Hessian and update computation - H = J' * J + mu * eye(size(J, 2)); - delta = -H \ (J' * r); -end \ No newline at end of file diff --git a/Other functions/ComputeJacobian.m b/Other functions/ComputeJacobian.m deleted file mode 100644 index 20cdade..0000000 --- a/Other functions/ComputeJacobian.m +++ /dev/null @@ -1,12 +0,0 @@ -function J = ComputeJacobian(P, Z) - % Decompose rows of the camera P - P1 = P(1, :); - P2 = P(2, :); - P3 = P(3, :); - - % Compute Jacobian (2x4) - J = [ - (Z(1) * P3 - Z(3) * P1 ) / Z(3)^2; - (Z(2) * P3 - Z(3) * P2 ) / Z(3)^2; - ]; -end diff --git a/Other functions/ComputeReprojectionError.m b/Other functions/ComputeReprojectionError.m deleted file mode 100644 index 39cc4dd..0000000 --- a/Other functions/ComputeReprojectionError.m +++ /dev/null @@ -1,18 +0,0 @@ -function [err, res] = ComputeReprojectionError(P1, P2, Xj, x1j, x2j) - % Project 3D point into both cameras - Z1 = P1 * Xj; - Z2 = P2 * Xj; - - % Compute residuals - r1 = x1j(1:2) - Z1(1:2) ./ Z1(3); % 2x1 - r2 = x2j(1:2) - Z2(1:2) ./ Z2(3); % 2x1 - res = [r1; r2]; % 4x1 - - % Compute total reprojection error - err = sum(res.^2); % 1x1 -end - - -%%% Confirmed - - diff --git a/Other functions/ComputeTotalError.m b/Other functions/ComputeTotalError.m deleted file mode 100644 index cdd56ff..0000000 --- a/Other functions/ComputeTotalError.m +++ /dev/null @@ -1,17 +0,0 @@ -function total_error = ComputeTotalError(P1, P2, X, x1, x2) - % Compute the total reprojection error for all 3D points - % - % Inputs: - % - P1, P2: Camera projection matrices - % - X: 4xN matrix of 3D points in homogeneous coordinates - % - x1, x2: 3xN matrices of 2D image points in homogeneous coordinates - % - % Outputs: - % - total_error: The total reprojection error across all points - - total_error = 0; % Initialize total error - for j = 1:size(X, 2) - [err, ~] = ComputeReprojectionError(P1, P2, X(:, j), x1(:, j), x2(:, j)); - total_error = total_error + err; - end -end \ No newline at end of file diff --git a/Other functions/ComputeUpdate.m b/Other functions/ComputeUpdate.m deleted file mode 100644 index 7af29be..0000000 --- a/Other functions/ComputeUpdate.m +++ /dev/null @@ -1,9 +0,0 @@ -function delta_Xj = ComputeUpdate(r, J, mu) - % Compute normal equations for the update - H = J.' * J + mu * eye(size(J, 2)); % Hessian approximation (4x4) - g = J.' * r; % Gradient (4x1) - - % Compute the update step - delta_Xj = -H \ g; % inv(H) -% Computes the LM update . -end \ No newline at end of file diff --git a/Other functions/LevenbergMarquardt.m b/Other functions/LevenbergMarquardt.m deleted file mode 100644 index f4559d8..0000000 --- a/Other functions/LevenbergMarquardt.m +++ /dev/null @@ -1,64 +0,0 @@ -function [X_refined, accumulated_error] = LevenbergMarquardt(P1, P2, X, x1, x2, mu, max_iterations, tolerance) - % Levenberg-Marquardt optimization for 3D point refinement - - % Temporary variable for updates - X_temp = X; - accumulated_error = zeros(1, max_iterations); % For plotting error - - for iter = 1:max_iterations - % disp(['Iteration Index: ', num2str(iter)]); - - % Compute total error before update - total_error_before = 0; - for j = 1:size(X_temp, 2) - [err, ~] = ComputeReprojectionError(P1, P2, X_temp(:, j), x1(:, j), x2(:, j)); - total_error_before = total_error_before + err; - end - accumulated_error(iter) = total_error_before; % Store error for plotting - % disp(['Total Error Before: ', num2str(total_error_before)]); - - % Update 3D points - for j = 1:size(X_temp, 2) - % Compute residual and Jacobian for point Xj - [r, J] = LinearizeReprojErr(P1, P2, X_temp(:, j), x1(:, j), x2(:, j)); - - % Compute LM update - delta_Xj = ComputeUpdate(r, J, mu); - - % Update 3D point (temporarily) - X_test = pflat(X_temp(:, j) + delta_Xj); - - % Compute individual errors - [err_before, ~] = ComputeReprojectionError(P1, P2, X_temp(:, j), x1(:, j), x2(:, j)); - [err_after, ~] = ComputeReprojectionError(P1, P2, X_test, x1(:, j), x2(:, j)); - - % Apply update if error improves - if err_after < err_before - X_temp(:, j) = X_test; - end - end - - % Compute total error after update - total_error_after = 0; - for j = 1:size(X_temp, 2) - [err, ~] = ComputeReprojectionError(P1, P2, X_temp(:, j), x1(:, j), x2(:, j)); - total_error_after = total_error_after + err; - end - % disp(['Total Error After: ', num2str(total_error_after)]); - - % Adjust damping factor and check for convergence - if total_error_after < total_error_before - mu = mu / 2; % Decrease damping factor - else - mu = mu * 2; % Increase damping factor - end - - if abs(total_error_after - total_error_before) < tolerance - % disp('Converged!'); - break; - end - end - - % Return refined points and accumulated errors - X_refined = X_temp; -end \ No newline at end of file diff --git a/Other functions/LinearizeReprojErr.m b/Other functions/LinearizeReprojErr.m deleted file mode 100644 index 649ef14..0000000 --- a/Other functions/LinearizeReprojErr.m +++ /dev/null @@ -1,17 +0,0 @@ -function [r, J] = LinearizeReprojErr(P1, P2, Xj, x1j, x2j) - % Project 3D point into both cameras - Z1 = P1 * Xj; - Z2 = P2 * Xj; - - % Compute residuals - r1 = x1j(1:2) - Z1(1:2) ./ Z1(3); % 2x1 - r2 = x2j(1:2) - Z2(1:2) ./ Z2(3); % 2x1 - r = [r1; r2]; % 4x1 - - % Compute Jacobians using chain rule - J1 = ComputeJacobian(P1, Z1); % 2x4 - J2 = ComputeJacobian(P2, Z2); % 2x4 - - % Combine Jacobians - J = [J1; J2]; % 4x4 -end \ No newline at end of file diff --git a/Other functions/arrangefigures.m b/Other functions/arrangefigures.m deleted file mode 100644 index d74ac8f..0000000 --- a/Other functions/arrangefigures.m +++ /dev/null @@ -1,182 +0,0 @@ -function arrangefigures(varargin) -% arrangeFigures automatically aranges open figures in a grid on the -% designated screen monitor. Options are: -% -% arrangefigures -% arrangefigures(monitor) -% arrangefigures(ontop) -% arrangefigures(monitor, fRatio) -% arrangefigures(monitor, [w h]) -% arrangefigures(monitor, fRatio, animate) -% -% -% Possible settings are: -% -% monitor: -% - default: 1 -% - 0 means all monitors -% - scalar input means specified monitor -% - array input means multiple chosen monitors. Monitors must be successive -% ontop: -% - if there is only one input argument and the input argument is of type -% logical and is set to true, then all active figures are placed on top. -% Everything else is left unchanged. -% fRatio: -% - default: 3/4 -% - set the figure ratio in height/width (default: 3/4) -% [h w]: -% - default: -> default is fRatio -% - set a fixed width (w) and height (h) in pixels -% animate: -% - default: false -% - boolean flag, added for fun, don't expect too much... Can also be of -% practical use when all figures needs to be place on top. -% -% (c) M. van Dijk - -if nargin > 3 - warning('Incorrect number of input arguments. Leaving figures unchanged.') - return -elseif nargin == 0 - monitor = 1; - fRatio = 3/4; - animate = false; -elseif nargin == 1 - monitor = varargin{1}; - fRatio = 3/4; - animate = false; -elseif nargin == 2 - monitor = varargin{1}; - fRatio = varargin{2}; - animate = false; -elseif nargin == 3 - monitor = varargin{1}; - fRatio = varargin{2}; - animate = varargin{3}; -end - -if numel(fRatio) == 2 - fh = fRatio(2); - fw = fRatio(1); - fRatio = fh/fw; -end - -% monitor = 2; -% fRatio = 0.25; -% animate = false; -spacer = 30; - -% get figure handles -figureHandles = sort(get(0, 'Children')); -NFigures = numel(figureHandles); - -% if first argument is logical, only place all figures on top -if islogical(monitor) - for figureHandle = figureHandles' - figure(figureHandle); - end - return -end - -% get number of monitors and the resolutions -MonitorExtends = get(0, 'MonitorPositions'); -NMonitors = size(MonitorExtends, 1); -if isscalar(monitor) - if monitor == 0 - monitor = 1:NMonitors; - end -end - -% check if monitor-array exceeds number of monitors -if min(monitor) > NMonitors - error(['Can''t find assigned monitor: NMonitors = ' num2str(NMonitors) ', Set display monitor = [' num2str((monitor)) ']']) -end -if min(monitor) < 1 - warning(['Can''t find assigned monitor: NMonitors = ' num2str(NMonitors) ', Set display monitor = [' num2str((monitor)) ']. Skipping monitors < 0']) - monitor = monitor(monitor > 0); -end -if max(monitor) > NMonitors - warning(['Can''t find assigned monitor: NMonitors = ' num2str(NMonitors) ', Set display monitor = [' num2str((monitor)) ']. Skipping monitors > ' num2str(NMonitors)]) - monitor = monitor(monitor <= NMonitors); -end -if isempty(monitor) - error('Assign a correct monitor') -end - -UsedMonitorExtends = MonitorExtends(monitor, :); - -taskbarOffset = 100; -monitorArea = [min(UsedMonitorExtends(:, 1:2), [], 1) max(UsedMonitorExtends(:, 3:4), [], 1)]; -x = monitorArea(1); -y = monitorArea(2); -w = monitorArea(3) - x; -h = monitorArea(4) - taskbarOffset - y; % 100 offset for taskbar - -% determine size figures and grid -hSpacer = 75; -if ~exist('fw', 'var') - figureArea = 0; - for NRows = 1:NFigures - NCols = ceil(NFigures / NRows); - fh = (h - 2*spacer) / NRows - spacer - hSpacer; - fw = fh / fRatio; - if fw > ((w - 2*spacer) / NCols - spacer) - fw = (w - 2*spacer) / NCols - spacer; - fh = fw * fRatio; - end - - if figureArea < fh*fw - setNRows = NRows; - setNCols = NCols; - figureArea = fh*fw; - end - end - NRows = setNRows; - NCols = setNCols; - fh = (h - 2*spacer) / NRows - spacer - hSpacer; - fw = (w - 2*spacer) / NCols - spacer; - if (fh/fw > fRatio) - fh = fw * fRatio; - else - fw = fh / fRatio; - end -else - NCols = floor((w - 2*spacer) / (fw - spacer)); - NRows = floor((h - 2*spacer) / (fh - spacer)); - if (NCols * NRows < NFigures) - warning(['Cannot display ' num2str(NFigures) ' figures using [w h] = [' num2str(w) ' ' num2str(h) '] on monitor ' num2str(monitor) '. Leaving figures unchanged.']); - return - end -end - -fw = floor(repmat(fw, [1 NFigures])); -fh = floor(repmat(fh, [1 NFigures])); -fx = repmat(spacer+(0:NCols-1)*(fw(1)+spacer), [1, NRows]) + x; -fy = (h - fh(1)) - sort(reshape(repmat(spacer+(0:NRows-1)*(fh(1)+spacer+hSpacer), [NCols, 1]), 1, NCols*NRows), 'ascend') + y; - - -if animate - nsteps = 10; - posarr = zeros(NFigures, 4, nsteps); - for n = 1:NFigures - posarr(n, :, 1) = get(figureHandles(n), 'position'); - posarr(n, 1, :) = linspace(posarr(n, 1, 1), fx(n), nsteps); - posarr(n, 2, :) = linspace(posarr(n, 2, 1), fy(n), nsteps); - posarr(n, 3, :) = linspace(posarr(n, 3, 1), fw(n), nsteps); - posarr(n, 4, :) = linspace(posarr(n, 4, 1), fh(n), nsteps); - figure(figureHandles(n)); - end - for step = 1:nsteps - for n = 1:NFigures - set(figureHandles(n), 'position', squeeze(posarr(n, :, step))); - end - pause(0.007); - end -else - for n = 1:NFigures - set(figureHandles(n), 'position', [fx(n) fy(n) fw(n) fh(n)]); - end -end - - - diff --git a/Other functions/estimate_camera_DLT.m b/Other functions/estimate_camera_DLT.m deleted file mode 100644 index 6832d14..0000000 --- a/Other functions/estimate_camera_DLT.m +++ /dev/null @@ -1,58 +0,0 @@ -function P = estimate_camera_DLT(x,X) -% Calculate the camera matrix using DLT -% Inputs: x, X: 3x1 - -% Add a dimension to X to make it homogenous -X = [X; ones(1,length(X))]; % 4x1 - -% Define the matrix M -% Number of points -N = size(X, 2); -M = zeros(3*N,12+N); % Initialize the matrix - -% Fill the values as blocks -for i= 1:N - % First row in the block - M(3*i-2, 1:4) = X(:,i)'; - M(3*i-2, 12+i) = -x(1,i); - - % Second row in the block - M(3*i-1, 5:8) = X(:,i)'; - M(3*i-1, 12+i) = -x(2,i); - - % Third row in the block - M(3*i, 9:12) = X(:,i)'; - M(3*i, 12+i) = -x(3,i); - -end - -% Calculate the S, V, U of Singular value decomposition -[U,S,V] = svd(M); - -% Check the smallest singular value -diagonal_numbers = diag(S); % Smallest value in the diagonal of S - -smallest_singular_value = diagonal_numbers(end); - -disp('Smallest Singular Value:'); -disp(smallest_singular_value); - -% v is the last column of V -v = V(:, end); % N+12 -P = reshape(v(1:12), [4 3])'; % P is 3x4 - -% Check ||Mv|| -Mv_abs = norm(M * v); -disp('||Mv||:'); -disp(Mv_abs); % 0.0150653751103407 - - -% Check the determinant of the rotation matrix -A = P(1:3, 1:3); - -if det(A)<0 - P = -P; -end - - -end \ No newline at end of file diff --git a/Other functions/run_sfm.m b/Other functions/run_sfm.m deleted file mode 100644 index 346e86e..0000000 --- a/Other functions/run_sfm.m +++ /dev/null @@ -1,1297 +0,0 @@ - -% function [] = run_sfm(dataset_num, varargin) -% RUN_SFM - Structure from Motion Pipeline -%% -% Inputs: -% dataset_num - (Required) Dataset number to process. -% varargin - (Optional) Key-value pairs: -% 'plot_initial_rel_3D' (default: false) - Flag to plot initial relative 3D points. -% 'plot_initial_rel_3D_pair' (default: false) - Flag to plot initial -% relative 3D points of the initial pair. -% -% Example Usage: -% run_sfm(2) % Default, no plot++ -% run_sfm(2, 'plot_initial_rel_3D', true) % Enable plotting for all -% relative pairs -% run_sfm(2, 'plot_initial_rel_3D_pair', true) % Enable plotting for the -% relative pair - -% --- Parse Optional Arguments --- -% p = inputParser; % Create parser object -% addRequired(p, 'dataset_num'); % Required input -% addParameter(p, 'plot_initial_rel_3D', false, @(x) islogical(x)); % Optional parameter -% addParameter(p, 'plot_initial_rel_3D_pair', false, @(x) islogical(x)); % Optional parameter -% -% parse(p, dataset_num, varargin{:}); % Parse inputs - -% Extract parameters -% dataset_num = p.Results.dataset_num; -% plot_initial_rel_3D = p.Results.plot_initial_rel_3D; % Optional flag -% plot_initial_rel_3D_pair = p.Results.plot_initial_rel_3D_pair; % Optional flag -close all; clear; clc -dataset_num = 15; -plot_initial_rel_3D = false; -plot_initial_rel_3D_pair = false; - -global enableInfo; -global info_level; - -enableInfo = true; % Enable or disable info globally -info_level = 1; -%% - -% reset and load data -% clc; close all; -% Add the vl_toolbox to be accessible in the current directory -run('.\vlfeat-0.9.21-bin\vlfeat-0.9.21\toolbox\vl_setup') -% Get dataset info by providing dataset input - -% Control random number generator - to get consistant resutls -rng(42) - -%% Load the dataset data information -% disp("Step 0: Load the dataset data information") -info("Step 0: Load the dataset data information") - - -% Extract dataset information -[K, img_names, init_pair, pixel_threshold] = get_dataset_info(dataset_num); - -%% Step 1: Calculate the relative Orientation -% This step calculates the relative rotations (R_rel) -% between consecutive image pairs in the dataset. -% It also performs feature extraction, matches features, estimates the essential -% matrix using RANSAC, and triangulates 3D points. - -% disp("Step 1: Calculate the relative Rotation between image pairs") - - -info("Step 1: Calculate the relative Rotation between image pairs") - -% Number of images in the dataset -N = length(img_names); - -% Initialize containers(cells) for storing relative rotations -R_rel = cell(1, N-1); % Relative rotations for each image pair - -% Containers for descriptors and features -desc_im = cell(1,N); -feat_im = cell(1,N); -matches_im = cell(1,N-1); - -% Container(cell) for inlier indices after RANSAC -inliers_indices = cell(1,N-1); - -% Loop through image pairs to compute relative rotations -for n=1:N-1 - %%% Feature Extraction using SIFT %%% - - % Load images for the current and next view - im1 = imread(img_names{n}); - im2 = imread(img_names{n+1}); - - % Extract features and descriptors using SIFT for both images - [x1, x2, ~, fA, dA, fB, dB, matches_im{n}] = feature_extraction(im1, im2); - - - % vectors with dim as - % 3 x N; - % N-> Number of the points - x1_h = toHomogeneous(x1); - x2_h = toHomogeneous(x2); - - % Assign features and descriptors only once for each image - if n == 1 - % Store the first image's features and descriptors (processed once) - feat_im{n} = fA; - desc_im{n} = dA; - end - - % Store features and descriptors for the second image in the pair - feat_im{n+1} = fB; - desc_im{n+1} = dB; - - - %%% Estimate the Essential Matrix using RANSAC %%% - - % Define the pixel threhold - epipolar_threshold = pixel_threshold / K(1,1); % Normalize threshold - - % e.g: 0.000420430520853354 - - x1_h_n = K\x1_h; - x2_h_n = K\x2_h; - - % Estimate Essential Matrix robustly using RANSAC - [E_ransac, indices] = estimate_E_robust(x1_h_n, x2_h_n, epipolar_threshold); - - % Store inlier indices for later refinement - inliers_indices{n} = indices; - - % Triangulate the points 3D points using the relative pose and inliers - % and Check for Cheirality Condition to get P2 - - x1_h_n_in = x1_h_n(:,inliers_indices{n}); - x2_h_n_in = x2_h_n(:,inliers_indices{n}); - - [X,P_pair] = Cheirality_triangulate(x1_h_n_in,x2_h_n_in,E_ransac); - - validate_camera(P_pair{1}) - - validate_camera(P_pair{2}) - - - % Save relative rotation - - R_rel{n} = P_pair{2}(1:3, 1:3); - - - if plot_initial_rel_3D - % Optional: Step 5: Visualization - figure; - - plot3(X(1, :), X(2, :), X(3, :), '.', 'MarkerSize', 5); % Plot 3D points - - hold on; - - P_pair_un = CalibratedToUncalibrated(P_pair, K); - plotcams(P_pair_un); % Plot cameras - xlabel('x'); ylabel('y'); zlabel('z'); - title('Relative plot of 3D points'); - axis equal; % Equal scaling for axes - grid on; - hold off; - end -end - - -%% Step 2: Upgrade to Absolute Rotations -% disp("Step 2: Upgrade to Absolute Rotations") - -info("Step 2: Upgrade to Absolute Rotations") - - -% Initialize cell array for storing absolute rotations -R_abs_i = cell(1, N); % Absolute rotations for N cameras -R_abs_i{1} = eye(3); % First camera has identity rotation (reference) - -% Compute absolute rotations from relative rotations -for k = 1:N-1 - % Compute absolute rotation by chaining relative rotations - R_abs_i{k+1} = R_rel{k} * R_abs_i{k}; % Accumulate rotations - - % Display progress for debugging - % fprintf('Computed Absolute Rotation (Camera %d):\n', k+1); - info("Computed Absolute Rotation (Camera %d):\n",k+1); - disp(R_abs_i{k+1}); -end - -% Final output -% disp("Absolute Rotations Computed for all Cameras!"); -info("Absolute Rotations Computed for all Cameras!"); - - -% Verify orthogonality (R * R' = I) -% fprintf('\nVerifying Orthogonality of Rotations:\n'); -info("\nVerifying Orthogonality of Rotations:\n"); - -for k = 1:N - error_orthogonality = norm(R_abs_i{k} * R_abs_i{k}' - eye(3)); % Should be close to 0 using Euclidean norm - info("Camera %d Orthogonality Error: %.6f\n", 1,k, error_orthogonality); -end - -%% Step 3: Reconstruct initial 3D points from initial image pair -% disp("Step 3: Reconstruct initial 3D points from initial image pair") - -info("Step 3: Reconstruct initial 3D points from initial image pair:\n"); - - -% get the 3D points from the suggested pair -% Load the images -im1 = imread(img_names{init_pair(1)}); -im2 = imread(img_names{init_pair(2)}); - -% Pipeline for extracting the featuers, - -[x1, x2, desc_X, ~,~,~,~,~] = feature_extraction(im1, im2); - - -% vectors with dim as -% 3 x N; -% N-> Number of the points -x1_h = toHomogeneous(x1); -x2_h = toHomogeneous(x2); - -x1_h_n = K\x1_h; -x2_h_n = K\x2_h; - -% Get the estimate_E_robust -[E_ransac, indices] = estimate_E_robust(x1_h_n, x2_h_n, epipolar_threshold); - - -% save('desc_X.mat', 'desc_X'); % Save to a mat file - -% Triangulate points using relative pose - -x1_h_n_in = x1_h_n(:,indices); -x2_h_n_in = x2_h_n(:,indices); - -[X_init_pair,P_init_pair] = Cheirality_triangulate(x1_h_n_in,x2_h_n_in,E_ransac); - -validate_camera(P_init_pair{1}) - -validate_camera(P_init_pair{2}) - -% X_init_pair = removePointsExcessivelyFarAway(X_init_pair); -% -% -% new_indices = find(indices < length(X_init_pair)); -% -% % Save descriptors for future use "Only inliers" -desc_X_init_pair = desc_X(:,indices); - - -% Move the 3D pionts to the 3D world coordinate -X_init_pair_wc = R_abs_i{init_pair(1)}' * X_init_pair(1:3, :); -X_init_pair_wc_h = toHomogeneous(X_init_pair_wc); - -%% -if true - % Step 5: Visualization for initial pair - figure; - plot3(X_init_pair_wc_h(1, :), X_init_pair_wc_h(2, :), X_init_pair_wc_h(3, :), "."); - hold on; - xlabel('x'); ylabel('y'); zlabel('z'); - title('Filtered 3D Points'); - xlim([-20, 20]); - ylim([-20, 20]); - zlim([-20, 20]); - hold off; -end - -%% -% Step 4: -% disp("Step 4: T robust estimation") -info("Step 4: T robust estimation:\n"); - - -% Establish correspondences between i and 3D points (using desc X), -% Number of images -% Initialize translation estimates -T_best = cell(1, N); -% T_best{1} = zeros(3, 1); - -for n = 1:N - % Step 1: Establish correspondences using descriptor matching - [matches_2d_3d, ~] = vl_ubcmatch(desc_X_init_pair, desc_im{n}); - - % unique_indices = unique(matches_2d_3d(2, :)); - % disp(['Unique 3D Points: ', num2str(length(unique_indices))]); - % disp(['Total Matches: ', num2str(size(matches_2d_3d, 2))]); - % info("Percentage of unique matches : %.2f%%\n: ", 100 * length(unique_indices) / size(matches_2d_3d, 2)); - - - % [~, ia] = unique(matches_2d_3d(2, :), 'stable'); - % filtered_matches = matches_2d_3d(:, ia); - - % Step 2: Extract 2D points and convert to homogeneous coordinates - xA = feat_im{n}(1:2, matches_2d_3d(2, :)); - - x1_h = toHomogeneous(xA); - - x1_h_n = K \ x1_h; % Normalize 2D points - - % Step 3: Extract 3D points and convert to homogeneous - Xs_h = X_init_pair_wc_h(:, matches_2d_3d(1, :)); % Use pre-computed 3D points - - % Step 4: Robust estimation of translation - translation_threshold = 3 * pixel_threshold / K(1, 1); - - disp("Determinant of R: ") - disp(det(R_abs_i{n})) - - % Estimate T using Robust RANSAC + DLT - %TODO: should we prodive the initial as zero ? zeros(3, 1) - T_best{n} = estimate_T_robust(x1_h_n, Xs_h, R_abs_i{n}, translation_threshold); - - - % Normalize it to avoid scale issues - % T_best{n+1} = T_best{n+1} / norm(T_best{n+1}); -end - -%% -% disp("Step 5: Plot the cameras") - -info("Step 5: Plot the cameras: \n: "); - - -P_all = cell(1,N); - -for i=1:N - P_all{i} = [R_abs_i{i} T_best{i}]; -end - -figure(); - -P_all_un = CalibratedToUncalibrated(P_all, K); -plotcams(P_all_un); - -title('Camera Poses Visualization'); - -% disp("Cameras plotted successfully!"); -% info("Cameras plotted successfully!: \n: ", 100 * length(unique_indices) / size(matches_2d_3d, 2)); - -%% - -% Step 6: Triangulate points for all pairs (i, i + 1) and visualize 3D points + cameras - -% disp("% Step 6: Triangulate points for all pairs (i, i + 1) and visualize 3D points + cameras") -info("Step 6: Triangulate points for all pairs (i, i + 1) and visualize 3D points + cameras: \n: "); - -X_all = []; % To store all 3D points - -% Generate a colormap with N-1 distinct colors -colormap = lines(N-1); - -% Initialize a cell array to store 3D points and colors -points_with_colors = cell(N-1, 1); - -for n = 1:N-1 - % Compute the matches - % matches = vl_ubcmatch(desc_im{n}, desc_im{n+1}); - - % Get the 2D points correspondences on each images' pair - x1 = feat_im{n}(1:2, matches_im{n}(1, :)); - x2 = feat_im{n+1}(1:2, matches_im{n}(2, :)); - - % Convert the vectors to homogeneous for this pair of images - x1_h = toHomogeneous(x1); - x2_h = toHomogeneous(x2); - - % Normalize the points - x1_h_n = K \ x1_h; - x2_h_n = K \ x2_h; - - % Estimate the Essential Matrix and get inliers - [~, indices] = estimate_E_robust(x1_h_n, x2_h_n, epipolar_threshold); - - % Keep only inliers - x1_h_n_in = x1_h_n(:, indices); - x2_h_n_in = x2_h_n(:, indices); - - % Triangulate the 3D points using cameras and the 2D points - X = triangulate_3D_point_DLT(x1_h_n_in, x2_h_n_in, P_all{n}, P_all{n+1}); - - % Remove excessively far away points for clarity - X = removePointsExcessivelyFarAway(X); - - % Save points and corresponding color in the cell array - points_with_colors{n} = struct('points', X, 'color', colormap(n, :)); -end - -%% -% Visualize all points with their colors - -% Visualize all points with their colors -figure(); -for n = 1:N-1 - % Extract points and color - X = points_with_colors{n}.points; - color = points_with_colors{n}.color; - - % Plot the points with the corresponding color - plot3(X(1, :), X(2, :), X(3, :), ".", 'Color',color); - hold on; -end - - -% Plot cameras -plotcams(P_all_un); - -% Labels and axes -xlabel('X'); -ylabel('Y'); -zlabel('Z'); -title('3D Points and Cameras - All Cameras'); -axis equal; -grid off; -hold off; - - - -% Save the structured data -save('points_with_colors.mat', 'points_with_colors'); - -% Save the required data -filename = sprintf('sfm_data_%d.mat', dataset_num); -save(filename, 'X', 'P_all', 'R_abs_i', 'T_best', 'inliers_indices'); -imagename = sprintf('dataset_image_%d.png', dataset_num); -saveas(gcf, imagename); % Save as PNG - - -%% Feature Extraction Function %% -function [x1,x2, desc_X, fA, dA, fB, dB, matches] = feature_extraction(im1,im2) - % FEATURE_EXTRACTION - Extracts keypoints, descriptors, and matches between two images. - % - % Inputs: - % im1, im2 - Input images (grayscale or RGB). - % - % Outputs: - % x1, x2 - 2D coordinates of matched keypoints in both images (2xN). - % desc_X - Descriptors of matched keypoints in the first image (128xN). - % fA, dA - Keypoints and descriptors for the first image. - % fB, dB - Keypoints and descriptors for the second image. - - - % Return a 2D corresponces and other .... - - % Extract the features using SIFT - % 1 specifies a threshold for rejecting low-contrast keypoints detected by SIFT. - - [fA, dA] = vl_sift(single(rgb2gray(im1)),'PeakThresh', 2); - [fB, dB] = vl_sift(single(rgb2gray(im2)),'PeakThresh', 2); - - % Compute the matches - matches = vl_ubcmatch(dA, dB); - - % Get the 2D points correspondences on each images' pair - x1 = fA(1:2 , matches(1 ,:)); - x2 = fB(1:2 , matches(2 ,:)); - - - % Save descriptors for matched points (3D points will correspond to these) - % Extract descriptors only for matched points in image 1 - desc_X = dA(:, matches(1, :)); - -end - -%% estimate_E_robust - -function [E_best, indices] = estimate_E_robust(x1_h_n, x2_h_n, epipolar_threshold) - % Robustly estimates the essential matrix E using RANSAC. - - % x1: 3xN , N: Number of points - % x2: 3XN , N: Number of points - - global enableInfo; - - - % RANSAC parameters - max_iterations = 10000; - num_points = size(x1_h_n, 2); - - best_inlier_count = 0; - E_best = []; - inliers = false(1, num_points); - - % RANSAC loop - for iter = 1:max_iterations - % Randomly sample 8 points correspondences - sample_indices = randperm(num_points, 8); - - x1_h_n_sample = x1_h_n(:, sample_indices); - x2_h_n_sample = x2_h_n(:, sample_indices); - - % Estimate Essential matrix - E_candidate = estimate_F_DLT(x1_h_n_sample, x2_h_n_sample); - E_candidate = enforce_essential(E_candidate); - - %Just for validation - validate_E(E_candidate) - - % Compute epipolar errors for all points - distances_l2_x2 = compute_epipolar_errors(E_candidate, x1_h_n, x2_h_n); - distances_l1_x1 = compute_epipolar_errors(E_candidate', x2_h_n, x1_h_n); - - % Symmetric epipolar error - errors = (distances_l2_x2.^2 + distances_l1_x1.^2) / 2; - - % Determine inliers - inliers_current = errors < epipolar_threshold^2; - num_inliers = sum(inliers_current); - - % Update best model - if num_inliers > best_inlier_count - best_inlier_count = num_inliers; - E_best = E_candidate; - inliers = inliers_current; - end - end - - % fprintf('Best inlier count: %d\n', best_inlier_count); - % fprintf('Percentage of inliers: %.2f%%\n', 100 * (best_inlier_count) / num_points); - - info("Percentage of inliers: %.2f \n: ", 100 * (best_inlier_count) / num_points); - - - % Return the indices of the inliners - indices = find(inliers); -end -%% - -function F_norm_esit = estimate_F_DLT(x1_h_n, x2_h_n) -% Function to compute the normalized fundamental matrix using the 8-point algorithm -% The output F will be normalized by frobenius norm - -% ESTIMATE_F_DLT - Computes the Fundamental Matrix using the 8-point algorithm. -% -% Inputs: -% x1s - 2D normalized points in the first image (3xN homogeneous coordinates). -% x2s - 2D normalized points in the second image (3xN homogeneous coordinates). -% -% Outputs: -% F_norm_esit - Estimated fundamental matrix (3x3) normalized by Frobenius norm. - -% Get the dimenion of the M matrix -n = length(x1_h_n); - -% Initialize M -M = zeros(n,9); - -% Loop over the points -for i=1:n - mul = x2_h_n(:,i) * x1_h_n(:,i)'; - mul = reshape(mul.', [], 1); - mul = mul'; - M(i,:) = mul; -end - -% Calculate the S, V, U of Singular value decomposition -[~,~,V] = svd(M); - -% v is the last column of V -v = V(:, end); % 9 elements - -% disp('||Mv||:'); disp(Mv_abs); - -F_norm_esit = reshape(v,[3, 3])'; -end -%% - -function E = enforce_essential(E_approx) -% ENFORCE_ESSENTIAL Apply the enforce on E - -% Apply the SVD on E approximate -[U,~,V] = svd( E_approx ); - -% if det(U*V') < 0 -% V = -V; -% end - -% Check the rank of F -rank_E_approx = rank(E_approx); - -% Display the rank -% disp(['Rank of E_approx: ', num2str(rank_E_approx)]); - -info("Rank of E_approx: %.2f \n: ", 2, rank_E_approx); - - -% Creates a valid essential matrix from an approximate solution - -E = U * diag ([1 1 0])* V'; - -% Check the rank of F -rank_E = rank(E); - -% Display the rank -% disp(['Rank of E: ', num2str(rank_E)]); -info("Rank of E: %.2f \n: ", 2, rank_E); - - -end - -%% -function distances = compute_epipolar_errors(F, x1_h_n, x2_h_n) - % Compute epipolar errors for a given fundamental matrix F and point correspondences - % - % Inputs: - % F - Fundamental matrix (3x3) - % x1_h_n - Points in the first image (3xN, homogeneous coordinates) and - % normalized - % x2_h_n - Points in the second image (3xN, homogeneous coordinates) and - % normalized - % - % Output: - % distances - Vector of epipolar distances for each point correspondence - % Compute epipolar lines for points in the second image - l = F * x1_h_n; % Epipolar lines in the second image - - % Normalize the epipolar lines to have unit norm - l = l ./ sqrt(l(1, :).^2 + l(2, :).^2); - - % Compute distances from points in the second image to their epipolar lines - distances = abs(sum(l .* x2_h_n, 1)); - -end -%% - -function [X_best, P] = Cheirality_triangulate(x1_h_n_in, x2_h_n_in, E_ransac) -% CHEIRALITY_TRIANGULATE - Triangulate 3D points and resolve E ambiguity -% Inputs: -% x1, x2 - 2D unnormalized homogeneous points in images 1 and 2 (3xN) -% inliers - Indices of inlier matches after RANSAC -% K - Intrinsic camera matrix (3x3) -% E_ransac - Estimated essential matrix (3x3) -% Outputs: -% X_best - Triangulated 3D points (4xN homogeneous coordinates) -% P - Correct camera projection matrices {P1, P2} - - % Use the selected inlier points for triangulation - - % Initialize best parameters - P2_correct_ind = 0; % Index for correct P2 - max_points_1 = 0; % Maximum valid points satisfying chirality - X_best = []; % Store the best 3D points - - % Step 2: Define camera matrices - % First camera at the origin (canonical form) - P1 = [eye(3), zeros(3, 1)]; - - % Step 3: Decompose Essential Matrix (E) to get R, t candidates - [U, ~, V] = svd(E_ransac); - W = [0 -1 0; 1 0 0; 0 0 1]; % Pre-defined rotation matrix - - % Four possible solutions for [R, t] - R1 = U * W * V'; - R2 = U * W.'*V'; - - t = U(:, 3); - - % Ensure proper rotations (determinant = +1) - if det(R1) < 0, R1 = -R1; end - if det(R2) < 0, R2 = -R2; end - - % Step 4: Generate P2 candidates (Four cases) - P2_candidates = { - [R1, t], - [R1, -t], - [R2, t], - [R2, -t] - }; - - % Step 5: Evaluate each P2 candidate based on chirality condition - for i = 1:4 - P2 = P2_candidates{i}; - - % Triangulate 3D points using current P2 - X = triangulate_3D_point_DLT(x1_h_n_in, x2_h_n_in, P1, P2); - - % Project back to both views - x1_proj = P1 * X; - x2_proj = P2 * X; - - % Count points in front of both cameras (chirality check) - valid_points = (x1_proj(3,:) > 0) & (x2_proj(3,:) > 0); - num_valid = sum(valid_points); - - - % Keep the candidate with the most valid points - if num_valid > max_points_1 - max_points_1 = num_valid; - P2_correct_ind = i; - X_best = X; % (:, valid_points); % Store the best 3D points % Filter valid 3D points - end - end - - - % Step 6: Final output - Correct projection matrices - P{1} = P1; - P{2} = P2_candidates{P2_correct_ind}; - - fprintf('\n--- Cheirality_triangulate Validation Start ---\n'); - - % Print the selected solution - fprintf('Selected P2 Index: %d\n', P2_correct_ind); - fprintf('Number of Valid Points: %d out of %d\n', max_points_1, length(x1_h_n_in)); - - % Optional: Compute reprojection error - err1 = compute_reprojection_error_P(P{1}, X_best, x1_h_n_in); % (:, valid_points)); - err2 = compute_reprojection_error_P(P{2}, X_best, x2_h_n_in); % (:, valid_points)); - fprintf('Mean Reprojection Error: %.4f (Image 1), %.4f (Image 2)\n', mean(err1), mean(err2)); - - fprintf('\n--- Cheirality_triangulate Validation End ---\n'); -end - -%% -function X = triangulate_3D_point_DLT(x1, x2, P1, P2) - % INPUT: - % x1 - 2D points in the first image (3xN homogeneous coordinates) - % x2 - 2D points in the second image (3xN homogeneous coordinates) - % P1 - Camera projection matrix for the first image (3x4) - % P2 - Camera projection matrix for the second image (3x4) - % OUTPUT: - % X - Triangulated 3D points (4xN homogeneous coordinates) - - % Number of points given in x1 and x2 - N = size(x1, 2); - - X = zeros(4, N); - - % Loop through all points to get all 3D points - for i = 1:N - % Construct the A matrix for triangulation - A = [ - x1(1,i) * P1(3,:) - P1(1,:); - x1(2,i) * P1(3,:) - P1(2,:); - - x2(1,i) * P2(3,:) - P2(1,:); - x2(2,i) * P2(3,:) - P2(2,:); - ]; - - % Solve the homogeneous system using SVD as before and we need only - % V to extract v - - [~, ~, V] = svd(A); - X(:,i) = V(:,end); % Last column of V gives the solution ( last eigenvector) - end - - fprintf('\n--- triangulate_3D_point_DLT Validation Start ---\n'); - % Optional: Compute reprojection error - err1 = compute_reprojection_error_P(P1, X, x1); - err2 = compute_reprojection_error_P(P2, X, x2); - fprintf('Mean Reprojection Error: %.4f (Image 1), %.4f (Image 2)\n', mean(err1), mean(err2)); - - fprintf('\n--- triangulate_3D_point_DLT Validation End ---\n'); - - % Normalize the points to make them homogeneous (4th coordinate = 1) - X = X(1:4, :) ./ X(4, :); -end - -%% - -% Function: Robust Translation Estimation -function T_best = estimate_T_robust(xs, Xs, R, inlier_threshold) - - % T_init - T_init = zeros(3,1); - - % Initialization - max_inliers = 0; - N = size(xs, 2); - iterations = 10000; % RANSAC iterations - - for i = 1:iterations - % Sample minimal points (2 points for translation) - sample_indices = randperm(N, 2); - xs_sample = xs(:, sample_indices); - Xs_sample = Xs(:, sample_indices); - - % Estimate candidate T using DLT - T_candidate = estimate_translation_DLT(xs_sample, Xs_sample, R); - - %T_candidate = compute_translation(xs_sample, Xs_sample, R); - - % Compute reprojection errors - % % errors = compute_reprojection_error_P([R,T_candidate], Xs, xs); - errors = compute_reprojection_errors(xs, Xs, R, T_candidate); - - % Count inliers - inliers_candidate = errors < inlier_threshold; - num_inliers = sum(inliers_candidate); - - % Update best model - if num_inliers > max_inliers - max_inliers = num_inliers; - T_best = T_candidate; - inliers = inliers_candidate; - end - end - - fprintf('Percentage of inliers in T_robust: %.2f%%\n', 100 * (max_inliers) / N); - - - % Refine with inliers - if max_inliers > 0 - T_best = T_best; - else - warning('No inliers found. Returning initial translation.'); - T_best = T_init; - end - - errors = compute_reprojection_errors(xs, Xs, R, T_best); - - % Count inliers - inliers_candidate = errors.^2 < inlier_threshold.^2; - num_inliers = sum(inliers_candidate); - - fprintf('Percentage of inliers in T_robust best: %.2f%%\n', 100 * (num_inliers) / N); - -end - - - - -function T = estimate_translation_DLT(xs, Xs, R) - % Inputs: - % xs - 2D points in homogeneous coordinates (3xN) - % Xs - 3D points in homogeneous coordinates (4xN) - % R - Rotation matrix (3x3) - - % Extract the first two points - x1 = xs(:,1); - x2 = xs(:,2); - X1 = Xs(:,1); - X2 = Xs(:,2); - - % Construct matrix directly using skew-symmetric form - M = [ - skew(x1) * R * X1(1:3), skew(x1); - skew(x2) * R * X2(1:3), skew(x2) - ]; - - % Solve using SVD - [~, ~, V] = svd(M); - T = V(2:end, end) ./ V(1,4); % Extract translation vector (last column, rows 4-6) -end - -function S = skew(x) - % Constructs the skew-symmetric matrix for cross product - S = [ 0 -x(3) x(2); - x(3) 0 -x(1); - -x(2) x(1) 0 ]; -end - - -% Function: Compute Reprojection Errors -function errors = compute_reprojection_errors(xs, Xs, R, T) - % Project 3D points - projected = (R * Xs(1:3, :) + T); - projected = projected(1:2, :) ./ projected(3, :); % Normalize - - % Compute errors - errors = sqrt(sum((xs(1:2, :) - projected).^2, 1)); % Euclidean distance -end - - -function errors = compute_reprojection_error_P(P, X, x) -% COMPUTE_REPROJECTION_ERROR - Compute reprojection error for 3D points -% Inputs: -% P - Projection matrix (3x4) -% X - 3D points (4xN homogeneous) -% x - 2D points (3xN homogeneous) -% Outputs: -% errors - Reprojection errors (1xN) - - % Project 3D points to 2D - x_proj = P * X; - x_proj = x_proj ./ x_proj(3, :); % Normalize to homogeneous - - % Compute Euclidean distance error - errors = sqrt(sum((x_proj(1:2, :) - x(1:2, :)).^2, 1)); % Pixel errors -end - - -%% LM Helper Functions -function [X_refined, accumulated_error] = LevenbergMarquardt(P1, P2, X, x1, x2, mu, max_iterations, tolerance) - % Levenberg-Marquardt optimization for 3D point refinement - - % Temporary variable for updates - X_temp = X; - accumulated_error = zeros(1, max_iterations); % For plotting error - - for iter = 1:max_iterations - % disp(['Iteration Index: ', num2str(iter)]); - - % Compute total error before update - total_error_before = 0; - for j = 1:size(X_temp, 2) - [err, ~] = ComputeReprojectionError(P1, P2, X_temp(:, j), x1(:, j), x2(:, j)); - total_error_before = total_error_before + err; - end - accumulated_error(iter) = total_error_before; % Store error for plotting - % disp(['Total Error Before: ', num2str(total_error_before)]); - - % Update 3D points - for j = 1:size(X_temp, 2) - % Compute residual and Jacobian for point Xj - [r, J] = LinearizeReprojErr(P1, P2, X_temp(:, j), x1(:, j), x2(:, j)); - - % Compute LM update - delta_Xj = ComputeUpdate(r, J, mu); - - % Update 3D point (temporarily) - X_test = pflat(X_temp(:, j) + delta_Xj); - - % Compute individual errors - [err_before, ~] = ComputeReprojectionError(P1, P2, X_temp(:, j), x1(:, j), x2(:, j)); - [err_after, ~] = ComputeReprojectionError(P1, P2, X_test, x1(:, j), x2(:, j)); - - % Apply update if error improves - if err_after < err_before - X_temp(:, j) = X_test; - end - end - - % Compute total error after update - total_error_after = 0; - for j = 1:size(X_temp, 2) - [err, ~] = ComputeReprojectionError(P1, P2, X_temp(:, j), x1(:, j), x2(:, j)); - total_error_after = total_error_after + err; - end - % disp(['Total Error After: ', num2str(total_error_after)]); - - % Adjust damping factor and check for convergence - if total_error_after < total_error_before - mu = mu / 2; % Decrease damping factor - else - mu = mu * 2; % Increase damping factor - end - - if abs(total_error_after - total_error_before) < tolerance - % disp('Converged!'); - break; - end - end - - % Return refined points and accumulated errors - X_refined = X_temp; -end - - -function J = ComputeJacobian(P, Z) - % Decompose rows of the camera P - P1 = P(1, :); - P2 = P(2, :); - P3 = P(3, :); - - % Compute Jacobian (2x4) - J = [ - (Z(1) * P3 - Z(3) * P1 ) / Z(3)^2; - (Z(2) * P3 - Z(3) * P2 ) / Z(3)^2; - ]; -end - - -function [err, res] = ComputeReprojectionError(P1, P2, Xj, x1j, x2j) - % Project 3D point into both cameras - Z1 = P1 * Xj; - Z2 = P2 * Xj; - - % Compute residuals - r1 = x1j(1:2) - Z1(1:2) ./ Z1(3); % 2x1 - r2 = x2j(1:2) - Z2(1:2) ./ Z2(3); % 2x1 - res = [r1; r2]; % 4x1 - - % Compute total reprojection error - err = sum(res.^2); % 1x1 -end - - -%%% Confirmed - - -function total_error = ComputeTotalError(P1, P2, X, x1, x2) - % Compute the total reprojection error for all 3D points - % - % Inputs: - % - P1, P2: Camera projection matrices - % - X: 4xN matrix of 3D points in homogeneous coordinates - % - x1, x2: 3xN matrices of 2D image points in homogeneous coordinates - % - % Outputs: - % - total_error: The total reprojection error across all points - - total_error = 0; % Initialize total error - for j = 1:size(X, 2) - [err, ~] = ComputeReprojectionError(P1, P2, X(:, j), x1(:, j), x2(:, j)); - total_error = total_error + err; - end -end - -function delta_Xj = ComputeUpdate(r, J, mu) - % Compute normal equations for the update - H = J.' * J + mu * eye(size(J, 2)); % Hessian approximation (4x4) - g = J.' * r; % Gradient (4x1) - - % Compute the update step - delta_Xj = -H \ g; % inv(H) -% Computes the LM update . -end - - -function [r, J] = LinearizeReprojErr(P1, P2, Xj, x1j, x2j) - % Project 3D point into both cameras - Z1 = P1 * Xj; - Z2 = P2 * Xj; - - % Compute residuals - r1 = x1j(1:2) - Z1(1:2) ./ Z1(3); % 2x1 - r2 = x2j(1:2) - Z2(1:2) ./ Z2(3); % 2x1 - r = [r1; r2]; % 4x1 - - % Compute Jacobians using chain rule - J1 = ComputeJacobian(P1, Z1); % 2x4 - J2 = ComputeJacobian(P2, Z2); % 2x4 - - % Combine Jacobians - J = [J1; J2]; % 4x4 -end - - - - -%% Other helper functions - - -function [V] = pflat(V) -% Normalize the n points in P^(m-1) - -V = V(:,:) ./ V(end, :); - -end -%% -function plotcams(P) - % Function to plot camera positions and viewing directions - % Inputs: - % P - Cell array of camera projection matrices - - % Initialize arrays to store camera centers and directions - c = zeros(4, length(P)); % Camera centers - v = zeros(3, length(P)); % Viewing directions - - for i = 1:length(P) - c(:,i) = null(P{i}); % Compute camera center (homogeneous coordinates) - v(:,i) = P{i}(3, 1:3); % Viewing direction (3rd row of P) - end - - % Normalize homogeneous coordinates - c = c ./ repmat(c(4,:), [4 1]); % Convert to 3D by dividing by the last element - - % Plot cameras using quiver3 - quiver3(c(1,:), c(2,:), c(3,:), v(1,:), v(2,:), v(3,:), 'r-'); - hold on; - - - % %Label all cameras - % for i = 1:length(P) - % label = sprintf('Camera %d', i); - % text(c(1,i), c(2,i), c(3,i), label, 'FontSize', 1, 'Color', 'b'); - % end - - % Enhance the plot for clarity - xlabel('X'); - ylabel('Y'); - zlabel('Z'); - grid on; - axis equal; - hold off; -end -%% - -function v_h = toHomogeneous(v) -% TOHOMOGENEOUS - Converts a vector to homogeneous coordinates. -% -% Usage: -% v_h = toHomogeneous(v) -% -% Inputs: -% v - Input vector (NxM) where N is the dimensionality, and M is the number of points. -% -% Outputs: -% v_h - Output vector in homogeneous coordinates ((N+1)xM). -% -% Example: -% v = [1; 2; 3]; % 3D point -% v_h = toHomogeneous(v); % Convert to homogeneous -> [1; 2; 3; 1] -% - -% Validate input -if nargin < 1 - error('Input vector is required.'); -end - -% Add a row of ones to make it homogeneous -v_h = [v; ones(1, size(v, 2))]; - -end - -%% -function validate_E(E) -% VALIDATE_E - Validates the Essential matrix E based on rank, singular values, -% epipolar constraint, and determinant. -% -% Inputs: -% E - Essential matrix (3x3) -% x1 - Normalized homogeneous coordinates of points in image 1 (3xN) -% x2 - Normalized homogeneous coordinates of points in image 2 (3xN) -% -% Example: -% validate_E(E_candidate, x1_h_n_sample, x2_h_n_sample); - - % fprintf('\n--- Essential Matrix Validation Start ---\n'); - info("\n--- Essential Matrix Validation Start ---\n", 2); - - % 1. Rank Constraint - rank_E = rank(E); % Compute rank - % fprintf('Rank of E: %d (Expected: 2)\n', rank_E); - - info("Rank of E: %d (Expected: 2)\n: ", 2, rank_E); - - - - % 2. Singular Value Constraint - [U, S, V] = svd(E); % SVD - singular_values = diag(S); - % fprintf('Singular values of E: [%.6f, %.6f, %.6f]\n', singular_values); - - info("Singular values of E: [%.6f, %.6f, %.6f]\n", 2, singular_values); - - - if abs(S(1,1) - S(2,2)) < 1e-6 && S(3,3) < 1e-6 - % fprintf('Singular value constraint: Satisfied\n'); - info("Singular value constraint: Satisfied\n", 2); - - else - % fprintf('Singular value constraint: Not satisfied\n'); - info("Singular value constraint: Not satisfied\n", 2); - - end - - % 4. Determinant Check - det_E = det(E); % Compute determinant - % fprintf('Determinant of E: %.6f (Expected: Close to 0)\n', det_E); - info("Determinant of E: %.6f (Expected: Close to 0)\n", 2, det_E); - - - % Final Assessment - if rank_E == 2 && abs(S(1,1) - S(2,2)) < 1e-6 && S(3,3) < 1e-6 && abs(det_E) < 1e-6 - % fprintf('Validation Status: PASS - Essential matrix is valid.\n'); - info("Validation Status: PASS - Essential matrix is valid.\n", 2); - - else - % fprintf('Validation Status: FAIL - Essential matrix is invalid.\n'); - info("Validation Status: FAIL - Essential matrix is invalid.\n", 2); - end - - % fprintf('\n--- Essential Matrix Validation End ---\n'); - info("\n--- Essential Matrix Validation End ---\n", 2); - -end - -%% -function validate_camera(P) -% VALIDATE_CAMERA - Validates the projection matrix of a camera -% -% Inputs: -% P - 3x4 Camera projection matrix -% K - 3x3 Intrinsic parameter camera matrix -% Outputs: -% Prints validation results for R and T: -% - Orthogonality error of R -% - Determinant of R -% - Translation vector (T) and its norm - - fprintf('\n--- validate_camera Validation Start ---\n'); - - - % --- Extract Rotation (R) and Translation (T) --- - R = P(1:3, 1:3); % Rotation matrix - T = P(1:3, 4); % Translation vector - - % --- Validate Orthogonality of R --- - orthogonality_error = norm(R * R' - eye(3)); % Should be close to 0 - fprintf('R Orthogonality Error: %.6e\n', orthogonality_error); - - % --- Validate Determinant of R --- - det_R = det(R); % Should be close to +1 - fprintf('Determinant of R: %.6f\n', det_R); - - % --- Display Translation Information --- - fprintf('Translation Vector T: [%.4f, %.4f, %.4f]\n', T(1), T(2), T(3)); - fprintf('Norm of T: %.4f\n', norm(T)); - - % --- Additional Checks --- - % Warn if errors exceed acceptable thresholds - if orthogonality_error > 1e-6 - warning('R is not orthogonal. Check rotation matrix!'); - end - if abs(det_R - 1) > 1e-3 - warning('Determinant of R is not 1. Check rotation matrix!'); - end - - fprintf('\n--- validate_camera Validation End ---\n'); - -end - -%% - -function info(message, varargin) -% INFO - Custom function to display hierarchical information messages. -% Prints messages based on the global info_level and enableInfo settings. -% -% Usage: -% info('Message') % Defaults to Level 1 -% info('Message', 1) % Explicit Level 1 -% info('Message with %d', 1, 10) % Level 1 with variables -% info('Message with %f', 2, 3.14159) % Level 2 with variables -% -% Global Settings: -% - enableInfo: Toggle info messages (true/false). -% - info_level: Controls verbosity (1 = only Level 1, 2 = Level 1 and 2). - -% Access global variables -global enableInfo; -global info_level; - -% Set default level to 1 if not explicitly provided -if nargin < 2 || ~isnumeric(varargin{1}) % Check if level is missing - level = 1; % Default level - args = varargin; % Treat everything else as variables -else - level = varargin{1}; % Extract level - args = varargin(2:end); % Remaining are variables -end - -% Print messages based on levels -if enableInfo - % Print if the current level is less than or equal to the global level - if level <= info_level - fprintf('[INFO][Level %d]: ', level); - fprintf(message, args{:}); % Pass additional arguments - fprintf('\n'); - end -end -end - -%% -function X_clean = removePointsExcessivelyFarAway(X_dirty) - % remove points behind the camera - X_in_front = X_dirty(:,find(X_dirty(3,:)>0)); - - % remove points far away from center of gravity - cent = mean(X_in_front(1:3,:), 2); - dist = sqrt(sum((X_in_front(1:3,:) - cent).^2, 1)); - threshold = 4 * quantile(dist, 0.9); - ind_clean = dist < threshold; - X_clean = X_in_front(:, ind_clean); - -end - -%% -function P_un = CalibratedToUncalibrated(P_ca, K) - -N = length(P_ca); - -for i=1:N - P_un{i} = K * P_ca{i}; -end - -end - - -% function T = compute_translation(xs, Xs, R) -% x1 = xs(:,1); -% x2 = xs(:,2); -% -% X1 = Xs(:,1); -% X2 = Xs(:,2); -% -% x1_cross = [0, -x1(3), x1(2); x1(3), 0, -x1(1); -x1(2), x1(1), 0]; -% x2_cross = [0, -x2(3), x2(2); x2(3), 0, -x2(1); -x2(2), x2(1), 0]; -% -% M = [x1_cross*R*X1(1:3), x1_cross; x2_cross*R*X2(1:3), x2_cross]; -% -% [~, ~, V] = svd(M); -% T = V(2:end, end) ./ V(1,4); -% end \ No newline at end of file diff --git a/Other functions/test_estimate_translation_DLT.m b/Other functions/test_estimate_translation_DLT.m deleted file mode 100644 index 9fac0b2..0000000 --- a/Other functions/test_estimate_translation_DLT.m +++ /dev/null @@ -1,136 +0,0 @@ -% Generate synthetic data -N = 100; % Number of 3D points -X_gt = [rand(3, N) * 10; ones(1, N)]; % 3D points in homogeneous coordinates - -% Ground Truth Translation (T_gt) and Rotation (R_gt) -T_gt = [1; 2; 3]; % Ground truth translation -R_gt = eye(3); % Identity rotation (no rotation) - -% Intrinsics -K = eye(3); % Assume identity intrinsic matrix for simplicity - -% Project 3D points into 2D -x = K * [R_gt, T_gt] * X_gt; -x = x ./ x(3, :); % Normalize - -% Normalize points -x_norm = K \ x; - -sample_indices = randperm(N, 2); -xs_sample = x(:, sample_indices); -Xs_sample = X_gt(:, sample_indices); - -% Estimate Translation using DLT -T_est = estimate_translation_DLT(xs_sample, Xs_sample, R_gt); - -% Display results -disp('Estimated T:'); -disp(T_est); -disp('Ground Truth T:'); -disp(T_gt); - -% Compute translation error -error = norm(T_gt - T_est); -disp('Translation Error:'); -disp(error); - - - -function T = estimate_translation_DLT(x, X, R) - % Inputs: - % x - 2D normalized points in homogeneous coordinates (3xN) - % X - 3D points in homogeneous coordinates (4xN) - % R - Rotation matrix (3x3) - - % Outputs: - % T - Estimated translation vector (3x1) - - % Number of points - N = size(x, 2); - - % Initialize matrices A and b - A = []; - b = []; - - % Loop through each point - for i = 1:N - % Extract the 2D point (x) and 3D point (X) - xi = x(:, i); % 2D point in homogeneous coordinates - Xi = X(:, i); % 3D point in homogeneous coordinates - - % Create the linear system based on the equations in the image - A_i = [ - Xi(4), 0, 0, -xi(1); % First row - 0, Xi(4), 0, -xi(2); % Second row - 0, 0, Xi(4), -xi(3) % Third row - ]; - - % Append to the full matrix A - A = [A; A_i]; - - % Compute b using the rotation matrix R - b_i = -R * Xi(1:3); % Projected 3D point - b = [b; b_i]; - end - - % Solve the linear system A * T = b using least squares - T_lambda = A \ b; - - % Extract translation vector T (first 3 elements) - T = T_lambda(1:3); - - % Compute residual error - residual = norm(A * T_lambda - b); % ||A * T_lambda - b|| - disp('Residual Error:'); - disp(residual); - - % Flip sign if necessary based on direction consistency - if dot(T, mean(X(1:3, :), 2)) < 0 - T = -T; % Flip the translation vector - end -end - -%% -% Load example images -img1 = imread("data/2/DSC_0025.JPG"); % Replace with your image -img2 = imread("data/2/DSC_0033.JPG"); % Replace with your image - -% Detect features in both images -gray1 = rgb2gray(img1); -gray2 = rgb2gray(img2); -points1 = detectSURFFeatures(gray1); -points2 = detectSURFFeatures(gray2); - -% Extract features -[features1, validPoints1] = extractFeatures(gray1, points1); -[features2, validPoints2] = extractFeatures(gray2, points2); - -% Match features -indexPairs = matchFeatures(features1, features2); -matchedPoints1 = validPoints1(indexPairs(:, 1)); -matchedPoints2 = validPoints2(indexPairs(:, 2)); - -% Define camera intrinsics -focalLength = [2.3785e+03, 2.3785e+03]; % Replace with actual values -principalPoint = [968, 648]; % Replace with actual values -imageSize = size(gray1); -intrinsics = cameraIntrinsics(focalLength, principalPoint, imageSize(1:2)); - -% Estimate the Essential matrix -[E, inliers] = estimateEssentialMatrix(... - matchedPoints1, matchedPoints2, intrinsics); - -% Select inlier matches -inlierPoints1 = matchedPoints1(inliers); -inlierPoints2 = matchedPoints2(inliers); - -% Recover relative camera pose -[R, T] = relativeCameraPose(E, intrinsics, inlierPoints1, inlierPoints2); - -% Display results -disp('Estimated Translation (T):'); -disp(T); - -% Display results -disp('Estimated Rotation (R):'); -disp(R); \ No newline at end of file diff --git a/Other functions/triangulate_3D_points.m b/Other functions/triangulate_3D_points.m deleted file mode 100644 index da3c24e..0000000 --- a/Other functions/triangulate_3D_points.m +++ /dev/null @@ -1,38 +0,0 @@ -function [X] = triangulate_3D_points(x1,x2, inliers, P1, P2) - - % Include only the inliers on the triangulation - x1_inliers = x1(:,inliers); - x2_inliers = x2(:,inliers); - - % Correct camera P2 - P2_correct_ind = 0 ; - max_points_1 = 0; - max_points_2 = 0; - - % X best - X_best = []; - - X = triangulate_3D_point_DLT(x1_inliers, x2_inliers,P1, P2); - x1_proj = P1 * X; - x2_proj = P2 * X; - - % This is a general condition for the position of the points - cond = sum((x1_proj(3,:) > 0) & ((x2_proj(3,:) > 0)) ); - - disp("Points in front of the camera") - disp(cond) - - % Plot the best camera with best 3D points - % figure(); - % plot3(X(1,:), X(2,:), X(3,:), '.', 'MarkerSize', 10); - % hold on; - % P{1} = P1; - % P{2} = P2; - % plotcams(P); - % - % xlabel('x'); ylabel('y'); zlabel('z'); - % title("Initial pair, 3D Reconstruction") - % axis equal; - % hold off; -end - diff --git a/README.md b/README.md new file mode 100644 index 0000000..851e8c8 --- /dev/null +++ b/README.md @@ -0,0 +1,63 @@ +[![View Structure-from-Motion-Algorithm on File Exchange](https://www.mathworks.com/matlabcentral/images/matlab-file-exchange.svg)](https://se.mathworks.com/matlabcentral/fileexchange/179169-structure-from-motion-algorithm) + +# Structure from Motion (SfM) Pipeline + +The `run_sfm` function implements a **Structure from Motion (SfM)** pipeline for reconstructing 3D structures from image datasets. It supports multiple options for visualization, logging, and dataset control, making it suitable for educational, research, and development purposes. + +--- + +## Function Overview + +The `run_sfm` function processes a specific dataset and performs the following tasks: +1. **Feature extraction** using SIFT. +2. **Estimation of relative camera poses** using robust methods like RANSAC. +3. **Triangulation** of 3D points. +4. **Optimization and reconstruction** of absolute camera poses and 3D points. +5. **Visualization of 3D structures and camera poses. + +--- + +## Function Signature + +```matlab +function [] = run_sfm(dataset_num, varargin) +``` +--- + +# How to Run the Code + +Follow the steps below to successfully execute the `run_sfm` function and process a dataset for Structure from Motion (SfM): + +--- + +## 1. **Prerequisites** +Before running the code, ensure you have the following: +- **MATLAB**: Installed and properly set up. +- **VLFeat Toolbox**: + - Download the toolbox from [VLFeat Website](http://www.vlfeat.org/). + - Extract it and make sure its path is correctly added to your MATLAB workspace. + - Example: Ensure the `vl_setup` script from VLFeat is accessible. + +--- + +## 2. **Setup** +1. **Add the Required Paths**: + - Ensure all necessary files and functions (e.g., `feature_extraction`, `estimate_E_robust`, `Cheirality_triangulate`) are in your MATLAB path. + - Run the `vl_setup` script to initialize VLFeat. + +2. **Prepare the Dataset**: + - Ensure the dataset files (e.g., images and camera calibration data) are in the required format and location. + - Each dataset must be supported by the `get_dataset_info` function, which extracts intrinsic camera parameters and image names. + +--- + +## 3. **Basic Execution** +Run the `run_sfm` function in MATLAB using the following syntax: + +```matlab +run_sfm(dataset_num); +``` +--- +## Sample of the results: + +![image](https://github.com/user-attachments/assets/03c75c94-62bf-4293-b61b-8694f0680bed) diff --git a/arrangefigures.m b/arrangefigures.m deleted file mode 100644 index d74ac8f..0000000 --- a/arrangefigures.m +++ /dev/null @@ -1,182 +0,0 @@ -function arrangefigures(varargin) -% arrangeFigures automatically aranges open figures in a grid on the -% designated screen monitor. Options are: -% -% arrangefigures -% arrangefigures(monitor) -% arrangefigures(ontop) -% arrangefigures(monitor, fRatio) -% arrangefigures(monitor, [w h]) -% arrangefigures(monitor, fRatio, animate) -% -% -% Possible settings are: -% -% monitor: -% - default: 1 -% - 0 means all monitors -% - scalar input means specified monitor -% - array input means multiple chosen monitors. Monitors must be successive -% ontop: -% - if there is only one input argument and the input argument is of type -% logical and is set to true, then all active figures are placed on top. -% Everything else is left unchanged. -% fRatio: -% - default: 3/4 -% - set the figure ratio in height/width (default: 3/4) -% [h w]: -% - default: -> default is fRatio -% - set a fixed width (w) and height (h) in pixels -% animate: -% - default: false -% - boolean flag, added for fun, don't expect too much... Can also be of -% practical use when all figures needs to be place on top. -% -% (c) M. van Dijk - -if nargin > 3 - warning('Incorrect number of input arguments. Leaving figures unchanged.') - return -elseif nargin == 0 - monitor = 1; - fRatio = 3/4; - animate = false; -elseif nargin == 1 - monitor = varargin{1}; - fRatio = 3/4; - animate = false; -elseif nargin == 2 - monitor = varargin{1}; - fRatio = varargin{2}; - animate = false; -elseif nargin == 3 - monitor = varargin{1}; - fRatio = varargin{2}; - animate = varargin{3}; -end - -if numel(fRatio) == 2 - fh = fRatio(2); - fw = fRatio(1); - fRatio = fh/fw; -end - -% monitor = 2; -% fRatio = 0.25; -% animate = false; -spacer = 30; - -% get figure handles -figureHandles = sort(get(0, 'Children')); -NFigures = numel(figureHandles); - -% if first argument is logical, only place all figures on top -if islogical(monitor) - for figureHandle = figureHandles' - figure(figureHandle); - end - return -end - -% get number of monitors and the resolutions -MonitorExtends = get(0, 'MonitorPositions'); -NMonitors = size(MonitorExtends, 1); -if isscalar(monitor) - if monitor == 0 - monitor = 1:NMonitors; - end -end - -% check if monitor-array exceeds number of monitors -if min(monitor) > NMonitors - error(['Can''t find assigned monitor: NMonitors = ' num2str(NMonitors) ', Set display monitor = [' num2str((monitor)) ']']) -end -if min(monitor) < 1 - warning(['Can''t find assigned monitor: NMonitors = ' num2str(NMonitors) ', Set display monitor = [' num2str((monitor)) ']. Skipping monitors < 0']) - monitor = monitor(monitor > 0); -end -if max(monitor) > NMonitors - warning(['Can''t find assigned monitor: NMonitors = ' num2str(NMonitors) ', Set display monitor = [' num2str((monitor)) ']. Skipping monitors > ' num2str(NMonitors)]) - monitor = monitor(monitor <= NMonitors); -end -if isempty(monitor) - error('Assign a correct monitor') -end - -UsedMonitorExtends = MonitorExtends(monitor, :); - -taskbarOffset = 100; -monitorArea = [min(UsedMonitorExtends(:, 1:2), [], 1) max(UsedMonitorExtends(:, 3:4), [], 1)]; -x = monitorArea(1); -y = monitorArea(2); -w = monitorArea(3) - x; -h = monitorArea(4) - taskbarOffset - y; % 100 offset for taskbar - -% determine size figures and grid -hSpacer = 75; -if ~exist('fw', 'var') - figureArea = 0; - for NRows = 1:NFigures - NCols = ceil(NFigures / NRows); - fh = (h - 2*spacer) / NRows - spacer - hSpacer; - fw = fh / fRatio; - if fw > ((w - 2*spacer) / NCols - spacer) - fw = (w - 2*spacer) / NCols - spacer; - fh = fw * fRatio; - end - - if figureArea < fh*fw - setNRows = NRows; - setNCols = NCols; - figureArea = fh*fw; - end - end - NRows = setNRows; - NCols = setNCols; - fh = (h - 2*spacer) / NRows - spacer - hSpacer; - fw = (w - 2*spacer) / NCols - spacer; - if (fh/fw > fRatio) - fh = fw * fRatio; - else - fw = fh / fRatio; - end -else - NCols = floor((w - 2*spacer) / (fw - spacer)); - NRows = floor((h - 2*spacer) / (fh - spacer)); - if (NCols * NRows < NFigures) - warning(['Cannot display ' num2str(NFigures) ' figures using [w h] = [' num2str(w) ' ' num2str(h) '] on monitor ' num2str(monitor) '. Leaving figures unchanged.']); - return - end -end - -fw = floor(repmat(fw, [1 NFigures])); -fh = floor(repmat(fh, [1 NFigures])); -fx = repmat(spacer+(0:NCols-1)*(fw(1)+spacer), [1, NRows]) + x; -fy = (h - fh(1)) - sort(reshape(repmat(spacer+(0:NRows-1)*(fh(1)+spacer+hSpacer), [NCols, 1]), 1, NCols*NRows), 'ascend') + y; - - -if animate - nsteps = 10; - posarr = zeros(NFigures, 4, nsteps); - for n = 1:NFigures - posarr(n, :, 1) = get(figureHandles(n), 'position'); - posarr(n, 1, :) = linspace(posarr(n, 1, 1), fx(n), nsteps); - posarr(n, 2, :) = linspace(posarr(n, 2, 1), fy(n), nsteps); - posarr(n, 3, :) = linspace(posarr(n, 3, 1), fw(n), nsteps); - posarr(n, 4, :) = linspace(posarr(n, 4, 1), fh(n), nsteps); - figure(figureHandles(n)); - end - for step = 1:nsteps - for n = 1:NFigures - set(figureHandles(n), 'position', squeeze(posarr(n, :, step))); - end - pause(0.007); - end -else - for n = 1:NFigures - set(figureHandles(n), 'position', [fx(n) fy(n) fw(n) fh(n)]); - end -end - - - diff --git a/Other functions/compute_epipolar_errors.m b/compute_epipolar_errors.m similarity index 56% rename from Other functions/compute_epipolar_errors.m rename to compute_epipolar_errors.m index da7de36..5829ba8 100644 --- a/Other functions/compute_epipolar_errors.m +++ b/compute_epipolar_errors.m @@ -1,21 +1,22 @@ -function distances = compute_epipolar_errors(F, x1s, x2s) - % Compute epipolar errors for a given fundamental matrix F and point correspondences - % - % Inputs: - % F - Fundamental matrix (3x3) - % x1s - Points in the first image (3xN, homogeneous coordinates) - % x2s - Points in the second image (3xN, homogeneous coordinates) - % - % Output: - % distances - Vector of epipolar distances for each point correspondence - % Compute epipolar lines for points in the second image - l = F * x1s; % Epipolar lines in the second image - - % Normalize the epipolar lines to have unit norm - l = l ./ sqrt(l(1, :).^2 + l(2, :).^2); - - % Compute distances from points in the second image to their epipolar lines - distances = abs(sum(l .* x2s, 1)); - -end - +function distances = compute_epipolar_errors(F, x1_h_n, x2_h_n) + % Compute epipolar errors for a given fundamental matrix F and point correspondences + % + % Inputs: + % F - Fundamental matrix (3x3) + % x1_h_n - Points in the first image (3xN, homogeneous coordinates) and + % normalized + % x2_h_n - Points in the second image (3xN, homogeneous coordinates) and + % normalized + % + % Output: + % distances - Vector of epipolar distances for each point correspondence + % Compute epipolar lines for points in the second image + l = F * x1_h_n; % Epipolar lines in the second image + + % Normalize the epipolar lines to have unit norm + l = l ./ sqrt(l(1, :).^2 + l(2, :).^2); + + % Compute distances from points in the second image to their epipolar lines + distances = abs(sum(l .* x2_h_n, 1)); + +end \ No newline at end of file diff --git a/compute_reprojection_error_P.m b/compute_reprojection_error_P.m new file mode 100644 index 0000000..6a38d14 --- /dev/null +++ b/compute_reprojection_error_P.m @@ -0,0 +1,16 @@ +function errors = compute_reprojection_error_P(P, X, x) +% COMPUTE_REPROJECTION_ERROR - Compute reprojection error for 3D points +% Inputs: +% P - Projection matrix (3x4) +% X - 3D points (4xN homogeneous) +% x - 2D points (3xN homogeneous) +% Outputs: +% errors - Reprojection errors (1xN) + + % Project 3D points to 2D + x_proj = P * X; + x_proj = x_proj ./ x_proj(3, :); % Normalize to homogeneous + + % Compute Euclidean distance error + errors = sqrt(sum((x_proj(1:2, :) - x(1:2, :)).^2, 1)); % Pixel errors +end \ No newline at end of file diff --git a/compute_reprojection_errors.m b/compute_reprojection_errors.m new file mode 100644 index 0000000..f6b0804 --- /dev/null +++ b/compute_reprojection_errors.m @@ -0,0 +1,9 @@ +% Function: Compute Reprojection Errors +function errors = compute_reprojection_errors(xs, Xs, R, T) + % Project 3D points + projected = (R * Xs(1:3, :) + T); + projected = projected(1:2, :) ./ projected(3, :); % Normalize + + % Compute errors + errors = sqrt(sum((xs(1:2, :) - projected).^2, 1)); % Euclidean distance +end diff --git a/enforce_essential.m b/enforce_essential.m new file mode 100644 index 0000000..aaeb4f0 --- /dev/null +++ b/enforce_essential.m @@ -0,0 +1,32 @@ +function E = enforce_essential(E_approx) +% ENFORCE_ESSENTIAL Apply the enforce on E + +% Apply the SVD on E approximate +[U,~,V] = svd( E_approx ); + +% if det(U*V') < 0 +% V = -V; +% end + +% Check the rank of F +rank_E_approx = rank(E_approx); + +% Display the rank +% disp(['Rank of E_approx: ', num2str(rank_E_approx)]); + +info("Rank of E_approx: %.2f \n: ", 2, rank_E_approx); + + +% Creates a valid essential matrix from an approximate solution + +E = U * diag ([1 1 0])* V'; + +% Check the rank of F +rank_E = rank(E); + +% Display the rank +% disp(['Rank of E: ', num2str(rank_E)]); +info("Rank of E: %.2f \n: ", 2, rank_E); + + +end \ No newline at end of file diff --git a/Helper functions/estimate_E_robust.m b/estimate_E_robust.m similarity index 55% rename from Helper functions/estimate_E_robust.m rename to estimate_E_robust.m index 44ac0c5..0413af3 100644 --- a/Helper functions/estimate_E_robust.m +++ b/estimate_E_robust.m @@ -1,52 +1,60 @@ -function [E_best, indices] = estimate_E_robust(x1, x2, eps, K) - % Robustly estimates the essential matrix E using RANSAC. - - % x1: 3xN , N: Number of points - % x2: 3XN , N: Number of points - - % Normalize points - x1_normalized = K\x1; - x2_normalized = K\x2; - - % RANSAC parameters - max_iterations = 10000; - num_points = size(x1, 2); - best_inlier_count = 0; - E_best = []; - inliers = false(1, num_points); - - % RANSAC loop - for iter = 1:max_iterations - % Randomly sample 8 points correspondences - sample_indices = randperm(num_points, 8); - - x1_sample = x1_normalized(:, sample_indices); - x2_sample = x2_normalized(:, sample_indices); - - % Estimate essential matrix - E_candidate = estimate_F_DLT(x1_sample, x2_sample); - E_candidate = enforce_essential(E_candidate); - - % Compute epipolar errors for all points - distances_l2_x2 = compute_epipolar_errors(E_candidate, x1_normalized, x2_normalized); - distances_l1_x1 = compute_epipolar_errors(E_candidate', x2_normalized, x1_normalized); - - % Symmetric epipolar error - errors = (distances_l2_x2.^2 + distances_l1_x1.^2) / 2; - - % Determine inliers - inliers_current = errors < eps^2; - num_inliers = sum(inliers_current); - - % Update best model - if num_inliers > best_inlier_count - best_inlier_count = num_inliers; - E_best = E_candidate; - inliers = inliers_current; - end - end - - fprintf('Best inlier count: %d\n', best_inlier_count); - fprintf('Percentage of inliers: %.2f%%\n', 100 * (best_inlier_count) / num_points); - indices = find(inliers); +function [E_best, indices] = estimate_E_robust(x1_h_n, x2_h_n, epipolar_threshold) + % Robustly estimates the essential matrix E using RANSAC. + + % x1: 3xN , N: Number of points + % x2: 3XN , N: Number of points + + global enableInfo; + + + % RANSAC parameters + max_iterations = 10000; + num_points = size(x1_h_n, 2); + + best_inlier_count = 0; + E_best = []; + inliers = false(1, num_points); + + % RANSAC loop + for iter = 1:max_iterations + % Randomly sample 8 points correspondences + sample_indices = randperm(num_points, 8); + + x1_h_n_sample = x1_h_n(:, sample_indices); + x2_h_n_sample = x2_h_n(:, sample_indices); + + % Estimate Essential matrix + E_candidate = estimate_F_DLT(x1_h_n_sample, x2_h_n_sample); + E_candidate = enforce_essential(E_candidate); + + %Just for validation + validate_E(E_candidate) + + % Compute epipolar errors for all points + distances_l2_x2 = compute_epipolar_errors(E_candidate, x1_h_n, x2_h_n); + distances_l1_x1 = compute_epipolar_errors(E_candidate', x2_h_n, x1_h_n); + + % Symmetric epipolar error + errors = (distances_l2_x2.^2 + distances_l1_x1.^2) / 2; + + % Determine inliers + inliers_current = errors < epipolar_threshold^2; + num_inliers = sum(inliers_current); + + % Update best model + if num_inliers > best_inlier_count + best_inlier_count = num_inliers; + E_best = E_candidate; + inliers = inliers_current; + end + end + + % fprintf('Best inlier count: %d\n', best_inlier_count); + % fprintf('Percentage of inliers: %.2f%%\n', 100 * (best_inlier_count) / num_points); + + info("Percentage of inliers: %.2f \n: ", 100 * (best_inlier_count) / num_points); + + + % Return the indices of the inliners + indices = find(inliers); end \ No newline at end of file diff --git a/Helper functions/estimate_F_DLT.m b/estimate_F_DLT.m similarity index 50% rename from Helper functions/estimate_F_DLT.m rename to estimate_F_DLT.m index c2acc2d..b04d162 100644 --- a/Helper functions/estimate_F_DLT.m +++ b/estimate_F_DLT.m @@ -1,31 +1,37 @@ -function F_norm_esit = estimate_F_DLT(x1s, x2s) -% Function to compute the normalized fundamental matrix using the 8-point algorithm -% The output F will be normalized by frobenius norm - -% Get the dimenion of the M matrix -n = length(x1s); - -% Initialize M -M = zeros(n,9); - -% Loop over the points -for i=1:n - mul = x2s(:,i) * x1s(:,i)'; - mul = reshape(mul.', [], 1); - mul = mul'; - M(i,:) = mul; -end - -% Calculate the S, V, U of Singular value decomposition -[~,~,V] = svd(M); - -% v is the last column of V -v = V(:, end); % 9 elements - -% disp('||Mv||:'); disp(Mv_abs); - -F_norm_esit = reshape(v,[3, 3])'; - -% F_norm_esit = F_norm_esit / norm(F_norm_esit, 'fro'); % Ask about this later? - -end +function F_norm_esit = estimate_F_DLT(x1_h_n, x2_h_n) +% Function to compute the normalized fundamental matrix using the 8-point algorithm +% The output F will be normalized by frobenius norm + +% ESTIMATE_F_DLT - Computes the Fundamental Matrix using the 8-point algorithm. +% +% Inputs: +% x1s - 2D normalized points in the first image (3xN homogeneous coordinates). +% x2s - 2D normalized points in the second image (3xN homogeneous coordinates). +% +% Outputs: +% F_norm_esit - Estimated fundamental matrix (3x3) normalized by Frobenius norm. + +% Get the dimenion of the M matrix +n = length(x1_h_n); + +% Initialize M +M = zeros(n,9); + +% Loop over the points +for i=1:n + mul = x2_h_n(:,i) * x1_h_n(:,i)'; + mul = reshape(mul.', [], 1); + mul = mul'; + M(i,:) = mul; +end + +% Calculate the S, V, U of Singular value decomposition +[~,~,V] = svd(M); + +% v is the last column of V +v = V(:, end); % 9 elements + +% disp('||Mv||:'); disp(Mv_abs); + +F_norm_esit = reshape(v,[3, 3])'; +end \ No newline at end of file diff --git a/estimate_T_robust.m b/estimate_T_robust.m new file mode 100644 index 0000000..2ee8ba0 --- /dev/null +++ b/estimate_T_robust.m @@ -0,0 +1,48 @@ +% Function: Robust Translation Estimation +function T_best = estimate_T_robust(xs, Xs, R, inlier_threshold) + + + % Initialization + max_inliers = 0; + N = size(xs, 2); + iterations = 10000; % RANSAC iterations + + for i = 1:iterations + % Sample minimal points (2 points for translation) + sample_indices = randperm(N, 2); + xs_sample = xs(:, sample_indices); + Xs_sample = Xs(:, sample_indices); + + % Estimate candidate T using DLT + T_candidate = estimate_translation_DLT(xs_sample, Xs_sample, R); + + % Compute reprojection errors + errors = compute_reprojection_errors(xs, Xs, R, T_candidate); + + % Count inliers + inliers_candidate = errors < inlier_threshold; + num_inliers = sum(inliers_candidate); + + % Update best model + if num_inliers > max_inliers + max_inliers = num_inliers; + T_best = T_candidate; + inliers = inliers_candidate; + end + end + + + % Refine with inliers + if max_inliers == 0 + warning('No inliers found. Returning initial translation.'); + T_best = zeros(3,1); + end + + errors = compute_reprojection_errors(xs, Xs, R, T_best); + + % Count inliers + inliers_candidate = errors < inlier_threshold; + num_inliers = sum(inliers_candidate); + + info('Percentage of inliers in T_robust: %.2f\n',2,100 * (num_inliers) / N); +end diff --git a/estimate_translation_DLT.m b/estimate_translation_DLT.m new file mode 100644 index 0000000..2c8974c --- /dev/null +++ b/estimate_translation_DLT.m @@ -0,0 +1,29 @@ +function T = estimate_translation_DLT(xs, Xs, R) + % Inputs: + % xs - 2D points in homogeneous coordinates (3xN) + % Xs - 3D points in homogeneous coordinates (4xN) + % R - Rotation matrix (3x3) + + % Extract the first two points + x1 = xs(:,1); + x2 = xs(:,2); + X1 = Xs(:,1); + X2 = Xs(:,2); + + % Construct matrix directly using skew-symmetric form + M = [ + skew(x1) * R * X1(1:3), skew(x1); + skew(x2) * R * X2(1:3), skew(x2) + ]; + + % Solve using SVD + [~, ~, V] = svd(M); + T = V(2:end, end) ./ V(1,4); % Extract translation vector (last column, rows 4-6) +end + +function S = skew(x) + % Constructs the skew-symmetric matrix for cross product + S = [ 0 -x(3) x(2); + x(3) 0 -x(1); + -x(2) x(1) 0 ]; +end \ No newline at end of file diff --git a/feature_extraction.m b/feature_extraction.m new file mode 100644 index 0000000..fd2219d --- /dev/null +++ b/feature_extraction.m @@ -0,0 +1,34 @@ +function [x1,x2, desc_X, fA, dA, fB, dB, matches] = feature_extraction(im1,im2) + % FEATURE_EXTRACTION - Extracts keypoints, descriptors, and matches between two images. + % + % Inputs: + % im1, im2 - Input images (grayscale or RGB). + % + % Outputs: + % x1, x2 - 2D coordinates of matched keypoints in both images (2xN). + % desc_X - Descriptors of matched keypoints in the first image (128xN). + % fA, dA - Keypoints and descriptors for the first image. + % fB, dB - Keypoints and descriptors for the second image. + + + % Return a 2D corresponces and other .... + + % Extract the features using SIFT + % 1 specifies a threshold for rejecting low-contrast keypoints detected by SIFT. + + [fA, dA] = vl_sift(single(rgb2gray(im1)),'PeakThresh', 2); + [fB, dB] = vl_sift(single(rgb2gray(im2)),'PeakThresh', 2); + + % Compute the matches + matches = vl_ubcmatch(dA, dB); + + % Get the 2D points correspondences on each images' pair + x1 = fA(1:2 , matches(1 ,:)); + x2 = fB(1:2 , matches(2 ,:)); + + + % Save descriptors for matched points (3D points will correspond to these) + % Extract descriptors only for matched points in image 1 + desc_X = dA(:, matches(1, :)); + +end \ No newline at end of file diff --git a/get_dataset_info.m b/get_dataset_info.m index 3bb7374..a93c157 100644 --- a/get_dataset_info.m +++ b/get_dataset_info.m @@ -89,10 +89,24 @@ focal_length_35mm = 38.0; % from the EXIF data init_pair = [4, 6]; pixel_threshold = 1.0; - elseif dataset == 10 - % Add your optional datasets... + elseif dataset == 10 + % Biblotek Chalmers + img_names = {"data/10/1.jpg", "data/10/2.jpg","data/10/3.jpg"}; + im_width = 2992; im_height = 2992; + focal_length_35mm = 23.0; % from the EXIF data + init_pair = [1, 3]; + pixel_threshold = 1.0; + % Restaurangen Chalmers + elseif dataset == 11 + % Triceratops model on a poster. + img_names = {"data/11/1.jpg", "data/11/2.jpg","data/11/3.jpg"}; + im_width = 2992; im_height = 2992; + focal_length_35mm = 23.0; % from the EXIF data + init_pair = [1, 3]; + pixel_threshold = 1.0; end + + focal_length = max(im_width, im_height) * focal_length_35mm / 35.0; + K = [focal_length 0 im_width/2; 0 focal_length im_height/2; 0 0 1]; - focal_length = max(im_width, im_height) * focal_length_35mm / 35.0 - K = [focal_length 0 im_width/2; 0 focal_length im_height/2; 0 0 1] end diff --git a/info.m b/info.m new file mode 100644 index 0000000..56bc508 --- /dev/null +++ b/info.m @@ -0,0 +1,37 @@ +function info(message, varargin) +% INFO - Custom function to display hierarchical information messages. +% Prints messages based on the global info_level and enableInfo settings. +% +% Usage: +% info('Message') % Defaults to Level 1 +% info('Message', 1) % Explicit Level 1 +% info('Message with %d', 1, 10) % Level 1 with variables +% info('Message with %f', 2, 3.14159) % Level 2 with variables +% +% Global Settings: +% - enableInfo: Toggle info messages (true/false). +% - info_level: Controls verbosity (1 = only Level 1, 2 = Level 1 and 2). + +% Access global variables +global enableInfo; +global info_level; + +% Set default level to 1 if not explicitly provided +if nargin < 2 || ~isnumeric(varargin{1}) % Check if level is missing + level = 1; % Default level + args = varargin; % Treat everything else as variables +else + level = varargin{1}; % Extract level + args = varargin(2:end); % Remaining are variables +end + +% Print messages based on levels +if enableInfo + % Print if the current level is less than or equal to the global level + if level <= info_level + fprintf('[INFO][Level %d]: ', level); + fprintf(message, args{:}); % Pass additional arguments + fprintf('\n'); + end +end +end diff --git a/keep_good_points.m b/keep_good_points.m new file mode 100644 index 0000000..9e978a7 --- /dev/null +++ b/keep_good_points.m @@ -0,0 +1,8 @@ +function X_good = keep_good_points(X_front) + % remove points far away from center of gravity + center_points = mean(X_front(1:3,:), 2); + distance_from_center = sqrt(sum((X_front(1:3,:) - center_points).^2, 1)); + threshold = 4 * quantile(distance_from_center, 0.9); + ind_clean = distance_from_center < threshold; + X_good = X_front(:, ind_clean); +end diff --git a/Helper functions/pflat.m b/pflat.m similarity index 94% rename from Helper functions/pflat.m rename to pflat.m index 5e4e4f6..01e2cad 100644 --- a/Helper functions/pflat.m +++ b/pflat.m @@ -1,7 +1,6 @@ - -function [V] = pflat(V) -% Normalize the n points in P^(m-1) - -V = V(:,:) ./ V(end, :); - +function [V] = pflat(V) +% Normalize the n points in P^(m-1) + +V = V(:,:) ./ V(end, :); + end \ No newline at end of file diff --git a/Helper functions/plotcams.m b/plotcams.m similarity index 80% rename from Helper functions/plotcams.m rename to plotcams.m index e359fc2..7a057c9 100644 --- a/Helper functions/plotcams.m +++ b/plotcams.m @@ -20,11 +20,11 @@ function plotcams(P) hold on; - %Label all cameras - for i = 1:length(P) - label = sprintf('Camera %d', i); - text(c(1,i), c(2,i), c(3,i), label, 'FontSize', 1, 'Color', 'b'); - end + % %Label all cameras + % for i = 1:length(P) + % label = sprintf('Camera %d', i); + % text(c(1,i), c(2,i), c(3,i), label, 'FontSize', 1, 'Color', 'b'); + % end % Enhance the plot for clarity xlabel('X'); diff --git a/points_in_front.m b/points_in_front.m new file mode 100644 index 0000000..b5743cb --- /dev/null +++ b/points_in_front.m @@ -0,0 +1,4 @@ +function X_front= points_in_front(X_points) + % remove points behind the camera + X_front = X_points(:,X_points(3,:)>0); +end \ No newline at end of file diff --git a/run_sfm.m b/run_sfm.m index 49df1a1..ba8be6a 100644 --- a/run_sfm.m +++ b/run_sfm.m @@ -1,47 +1,59 @@ - -% function [] = run_sfm(dataset_num, varargin) -% RUN_SFM - Structure from Motion Pipeline -%% +function [] = run_sfm(dataset_num, varargin) +% RUN_SFM - Structure from Motion (SfM) Pipeline +% +% This function processes a dataset for Structure from Motion, with optional +% visualization and logging control through key-value parameters. +% % Inputs: -% dataset_num - (Required) Dataset number to process. +% dataset_num - (Required) Dataset number to process (integer). % varargin - (Optional) Key-value pairs: -% 'plot_initial_rel_3D' (default: false) - Flag to plot initial relative 3D points. -% 'plot_initial_rel_3D_pair' (default: false) - Flag to plot initial -% relative 3D points of the initial pair. +% 'plot_initial_rel_3D' (default: false) - Enable plotting of initial +% relative 3D points for all pairs. +% 'plot_initial_rel_3D_pair' (default: false) - Enable plotting of initial +% relative 3D points for the first pair. +% 'enableInfo' (default: true) - Enable or disable informational messages. +% 'info_level' (default: 1) - Set informational detail level: +% 1 - Basic informational messages. +% 2 - Detailed execution logs. % % Example Usage: -% run_sfm(2) % Default, no plot++ -% run_sfm(2, 'plot_initial_rel_3D', true) % Enable plotting for all -% relative pairs -% run_sfm(2, 'plot_initial_rel_3D_pair', true) % Enable plotting for the -% relative pair +% run_sfm(2); % Default settings +% run_sfm(2, 'plot_initial_rel_3D', true); % Enable 3D plotting for all pairs +% run_sfm(2, 'enableInfo', false); % Disable informational messages +% run_sfm(2, 'info_level', 2); % Enable detailed execution logs +% +% Notes: +% - This function uses global variables 'enableInfo' and 'info_level'. +% - 'info_level' accepts values of 1 or 2 only. % --- Parse Optional Arguments --- -% p = inputParser; % Create parser object -% addRequired(p, 'dataset_num'); % Required input -% addParameter(p, 'plot_initial_rel_3D', false, @(x) islogical(x)); % Optional parameter -% addParameter(p, 'plot_initial_rel_3D_pair', false, @(x) islogical(x)); % Optional parameter -% -% parse(p, dataset_num, varargin{:}); % Parse inputs - -% Extract parameters -% dataset_num = p.Results.dataset_num; -% plot_initial_rel_3D = p.Results.plot_initial_rel_3D; % Optional flag -% plot_initial_rel_3D_pair = p.Results.plot_initial_rel_3D_pair; % Optional flag -close all; clear; clc -dataset_num = 1; -plot_initial_rel_3D = false; -plot_initial_rel_3D_pair = false; +p = inputParser; % Create input parser +addRequired(p, 'dataset_num', @(x) isnumeric(x) && isscalar(x)); % Dataset number +addParameter(p, 'plot_initial_rel_3D', false, @(x) islogical(x)); % Plotting flag +addParameter(p, 'plot_initial_rel_3D_pair', false, @(x) islogical(x)); % Initial pair plotting flag +addParameter(p, 'enableInfo', false, @(x) islogical(x)); % Enable or disable logs +addParameter(p, 'info_level', 1, @(x) isnumeric(x) && ismember(x, [1, 2])); % Log detail level (1 or 2) +parse(p, dataset_num, varargin{:}); % Parse inputs + +% --- Set Global Variables --- global enableInfo; global info_level; -enableInfo = true; % Enable or disable info globally -info_level = 1; -%% + +% --- Extract Parameters --- +dataset_num = p.Results.dataset_num; +plot_initial_rel_3D = p.Results.plot_initial_rel_3D; +plot_initial_rel_3D_pair = p.Results.plot_initial_rel_3D_pair; +enableInfo = p.Results.enableInfo; +info_level = p.Results.info_level; + + +% enableInfo = true; % Enable or disable info globally +% info_level = 1; % reset and load data -% clc; close all; + % Add the vl_toolbox to be accessible in the current directory run('.\vlfeat-0.9.21-bin\vlfeat-0.9.21\toolbox\vl_setup') % Get dataset info by providing dataset input @@ -49,23 +61,18 @@ % Control random number generator - to get consistant resutls rng(42) -%% Load the dataset data information -% disp("Step 0: Load the dataset data information") +% Load the dataset data information info("Step 0: Load the dataset data information") - % Extract dataset information [K, img_names, init_pair, pixel_threshold] = get_dataset_info(dataset_num); -%% Step 1: Calculate the relative Orientation +% Step 1: Calculate the relative Orientation % This step calculates the relative rotations (R_rel) % between consecutive image pairs in the dataset. % It also performs feature extraction, matches features, estimates the essential % matrix using RANSAC, and triangulates 3D points. -% disp("Step 1: Calculate the relative Rotation between image pairs") - - info("Step 1: Calculate the relative Rotation between image pairs") % Number of images in the dataset @@ -93,11 +100,10 @@ % Extract features and descriptors using SIFT for both images [x1, x2, ~, fA, dA, fB, dB, matches_im{n}] = feature_extraction(im1, im2); - % vectors with dim as - % 3 x N; + % 3 x N; % N-> Number of the points - x1_h = toHomogeneous(x1); + x1_h = toHomogeneous(x1); x2_h = toHomogeneous(x2); % Assign features and descriptors only once for each image @@ -110,10 +116,10 @@ % Store features and descriptors for the second image in the pair feat_im{n+1} = fB; desc_im{n+1} = dB; - + %%% Estimate the Essential Matrix using RANSAC %%% - + % Define the pixel threhold epipolar_threshold = pixel_threshold / K(1,1); % Normalize threshold @@ -130,17 +136,15 @@ % Triangulate the points 3D points using the relative pose and inliers % and Check for Cheirality Condition to get P2 - + x1_h_n_in = x1_h_n(:,inliers_indices{n}); x2_h_n_in = x2_h_n(:,inliers_indices{n}); - - [X,P_pair] = Cheirality_triangulate(x1_h_n_in,x2_h_n_in,E_ransac); - validate_camera(P_pair{1}) + [X,P_pair] = Cheirality_triangulate(x1_h_n_in,x2_h_n_in,E_ransac); - validate_camera(P_pair{2}) + validate_camera(P_pair{1}); + validate_camera(P_pair{2}); - % Save relative rotation R_rel{n} = P_pair{2}(1:3, 1:3); @@ -149,7 +153,6 @@ if plot_initial_rel_3D % Optional: Step 5: Visualization figure; - plot3(X(1, :), X(2, :), X(3, :), '.', 'MarkerSize', 5); % Plot 3D points hold on; @@ -165,12 +168,10 @@ end -%% Step 2: Upgrade to Absolute Rotations -% disp("Step 2: Upgrade to Absolute Rotations") +% Step 2: Upgrade to Absolute Rotations info("Step 2: Upgrade to Absolute Rotations") - % Initialize cell array for storing absolute rotations R_abs_i = cell(1, N); % Absolute rotations for N cameras R_abs_i{1} = eye(3); % First camera has identity rotation (reference) @@ -181,45 +182,43 @@ R_abs_i{k+1} = R_rel{k} * R_abs_i{k}; % Accumulate rotations % Display progress for debugging - % fprintf('Computed Absolute Rotation (Camera %d):\n', k+1); - info("Computed Absolute Rotation (Camera %d):\n",k+1); - disp(R_abs_i{k+1}); + info("Computed Absolute Rotation (Camera %d):\n",1,k+1); + info("Absolute camera",1,R_abs_i{k+1}); end % Final output -% disp("Absolute Rotations Computed for all Cameras!"); info("Absolute Rotations Computed for all Cameras!"); % Verify orthogonality (R * R' = I) -% fprintf('\nVerifying Orthogonality of Rotations:\n'); info("\nVerifying Orthogonality of Rotations:\n"); -for k = 1:N +for k = 1:N error_orthogonality = norm(R_abs_i{k} * R_abs_i{k}' - eye(3)); % Should be close to 0 using Euclidean norm info("Camera %d Orthogonality Error: %.6f\n", 1,k, error_orthogonality); end -%% Step 3: Reconstruct initial 3D points from initial image pair -% disp("Step 3: Reconstruct initial 3D points from initial image pair") +% Step 3: Reconstruct initial 3D points from initial image pair info("Step 3: Reconstruct initial 3D points from initial image pair:\n"); +% Get the 3D points from the suggested pair -% get the 3D points from the suggested pair -% Load the images -im1 = imread(img_names{init_pair(1)}); -im2 = imread(img_names{init_pair(2)}); +% Compute the matches +matches = vl_ubcmatch(desc_im{init_pair(1)}, desc_im{init_pair(2)}); -% Pipeline for extracting the featuers, - -[x1, x2, desc_X, ~,~,~,~,~] = feature_extraction(im1, im2); +% Get the 2D points correspondences on each images' pair +x1 = feat_im{init_pair(1)}(1:2 , matches(1 ,:)); +x2 = feat_im{init_pair(2)}(1:2 , matches(2 ,:)); +% Save descriptors for matched points (3D points will correspond to these) +% Extract descriptors only for matched points in image 1 +desc_X = desc_im{init_pair(1)}(:, matches(1, :)); % vectors with dim as -% 3 x N; +% 3 x N; % N-> Number of the points -x1_h = toHomogeneous(x1); +x1_h = toHomogeneous(x1); x2_h = toHomogeneous(x2); x1_h_n = K\x1_h; @@ -228,9 +227,6 @@ % Get the estimate_E_robust [E_ransac, indices] = estimate_E_robust(x1_h_n, x2_h_n, epipolar_threshold); - -% save('desc_X.mat', 'desc_X'); % Save to a mat file - % Triangulate points using relative pose x1_h_n_in = x1_h_n(:,indices); @@ -239,15 +235,12 @@ [X_init_pair,P_init_pair] = Cheirality_triangulate(x1_h_n_in,x2_h_n_in,E_ransac); validate_camera(P_init_pair{1}) - validate_camera(P_init_pair{2}) - % % Save descriptors for future use "Only inliers" desc_X_init_pair = desc_X(:,indices); - -% Move the 3D pionts to the 3D world coordinate +% Move the 3D pionts to the 3D world coordinate X_init_pair_wc = R_abs_i{init_pair(1)}' * X_init_pair(1:3, :); X_init_pair_wc_h = toHomogeneous(X_init_pair_wc); @@ -262,12 +255,9 @@ hold off; end -%% % Step 4: -% disp("Step 4: T robust estimation") info("Step 4: T robust estimation:\n"); - % Establish correspondences between i and 3D points (using desc X), % Number of images % Initialize translation estimates @@ -287,26 +277,24 @@ % Step 3: Extract 3D points and convert to homogeneous Xs_h = X_init_pair_wc_h(:, matches_2d_3d(1, :)); % Use pre-computed 3D points - + % Step 4: Robust estimation of translation translation_threshold = 3 * pixel_threshold / K(1, 1); - disp("Determinant of R: ") - disp(det(R_abs_i{n})) + info("Determinant of R: ",2,det(R_abs_i{n})); % Estimate T using Robust RANSAC + DLT T_best{n} = estimate_T_robust(x1_h_n, Xs_h, R_abs_i{n}, translation_threshold); end -%% -% disp("Step 5: Plot the cameras") +% Step 5: Plot the cameras: info("Step 5: Plot the cameras: \n: "); P_all = cell(1,N); for i=1:N - P_all{i} = [R_abs_i{i} T_best{i}]; + P_all{i} = [R_abs_i{i} T_best{i}]; end figure(); @@ -316,14 +304,9 @@ title('Camera Poses Visualization'); -% disp("Cameras plotted successfully!"); -% info("Cameras plotted successfully!: \n: ", 100 * length(unique_indices) / size(matches_2d_3d, 2)); - -%% +info("Cameras plotted successfully! "); % Step 6: Triangulate points for all pairs (i, i + 1) and visualize 3D points + cameras - -% disp("% Step 6: Triangulate points for all pairs (i, i + 1) and visualize 3D points + cameras") info("Step 6: Triangulate points for all pairs (i, i + 1) and visualize 3D points + cameras: \n: "); X_all = []; % To store all 3D points @@ -337,7 +320,7 @@ for n = 1:N-1 % Compute the matches % matches = vl_ubcmatch(desc_im{n}, desc_im{n+1}); - + % Get the 2D points correspondences on each images' pair x1 = feat_im{n}(1:2, matches_im{n}(1, :)); x2 = feat_im{n+1}(1:2, matches_im{n}(2, :)); @@ -367,9 +350,6 @@ points_with_colors{n} = struct('points', X, 'color', colormap(n, :)); end -%% -% Visualize all points with their colors - % Visualize all points with their colors figure(); for n = 1:N-1 @@ -403,852 +383,6 @@ imagename = sprintf('dataset_image_%d.png', dataset_num); saveas(gcf, imagename); % Save as PNG - -%% Feature Extraction Function %% -function [x1,x2, desc_X, fA, dA, fB, dB, matches] = feature_extraction(im1,im2) - % FEATURE_EXTRACTION - Extracts keypoints, descriptors, and matches between two images. - % - % Inputs: - % im1, im2 - Input images (grayscale or RGB). - % - % Outputs: - % x1, x2 - 2D coordinates of matched keypoints in both images (2xN). - % desc_X - Descriptors of matched keypoints in the first image (128xN). - % fA, dA - Keypoints and descriptors for the first image. - % fB, dB - Keypoints and descriptors for the second image. - - - % Return a 2D corresponces and other .... - - % Extract the features using SIFT - % 1 specifies a threshold for rejecting low-contrast keypoints detected by SIFT. - - [fA, dA] = vl_sift(single(rgb2gray(im1)),'PeakThresh', 2); - [fB, dB] = vl_sift(single(rgb2gray(im2)),'PeakThresh', 2); - - % Compute the matches - matches = vl_ubcmatch(dA, dB); - - % Get the 2D points correspondences on each images' pair - x1 = fA(1:2 , matches(1 ,:)); - x2 = fB(1:2 , matches(2 ,:)); - - - % Save descriptors for matched points (3D points will correspond to these) - % Extract descriptors only for matched points in image 1 - desc_X = dA(:, matches(1, :)); - -end - -%% estimate_E_robust - -function [E_best, indices] = estimate_E_robust(x1_h_n, x2_h_n, epipolar_threshold) - % Robustly estimates the essential matrix E using RANSAC. - - % x1: 3xN , N: Number of points - % x2: 3XN , N: Number of points - - global enableInfo; - - - % RANSAC parameters - max_iterations = 10000; - num_points = size(x1_h_n, 2); - - best_inlier_count = 0; - E_best = []; - inliers = false(1, num_points); - - % RANSAC loop - for iter = 1:max_iterations - % Randomly sample 8 points correspondences - sample_indices = randperm(num_points, 8); - - x1_h_n_sample = x1_h_n(:, sample_indices); - x2_h_n_sample = x2_h_n(:, sample_indices); - - % Estimate Essential matrix - E_candidate = estimate_F_DLT(x1_h_n_sample, x2_h_n_sample); - E_candidate = enforce_essential(E_candidate); - - %Just for validation - validate_E(E_candidate) - - % Compute epipolar errors for all points - distances_l2_x2 = compute_epipolar_errors(E_candidate, x1_h_n, x2_h_n); - distances_l1_x1 = compute_epipolar_errors(E_candidate', x2_h_n, x1_h_n); - - % Symmetric epipolar error - errors = (distances_l2_x2.^2 + distances_l1_x1.^2) / 2; - - % Determine inliers - inliers_current = errors < epipolar_threshold^2; - num_inliers = sum(inliers_current); - - % Update best model - if num_inliers > best_inlier_count - best_inlier_count = num_inliers; - E_best = E_candidate; - inliers = inliers_current; - end - end - - % fprintf('Best inlier count: %d\n', best_inlier_count); - % fprintf('Percentage of inliers: %.2f%%\n', 100 * (best_inlier_count) / num_points); - - info("Percentage of inliers: %.2f \n: ", 100 * (best_inlier_count) / num_points); - - - % Return the indices of the inliners - indices = find(inliers); -end -%% - -function F_norm_esit = estimate_F_DLT(x1_h_n, x2_h_n) -% Function to compute the normalized fundamental matrix using the 8-point algorithm -% The output F will be normalized by frobenius norm - -% ESTIMATE_F_DLT - Computes the Fundamental Matrix using the 8-point algorithm. -% -% Inputs: -% x1s - 2D normalized points in the first image (3xN homogeneous coordinates). -% x2s - 2D normalized points in the second image (3xN homogeneous coordinates). -% -% Outputs: -% F_norm_esit - Estimated fundamental matrix (3x3) normalized by Frobenius norm. - -% Get the dimenion of the M matrix -n = length(x1_h_n); - -% Initialize M -M = zeros(n,9); - -% Loop over the points -for i=1:n - mul = x2_h_n(:,i) * x1_h_n(:,i)'; - mul = reshape(mul.', [], 1); - mul = mul'; - M(i,:) = mul; -end - -% Calculate the S, V, U of Singular value decomposition -[~,~,V] = svd(M); - -% v is the last column of V -v = V(:, end); % 9 elements - -% disp('||Mv||:'); disp(Mv_abs); - -F_norm_esit = reshape(v,[3, 3])'; -end -%% - -function E = enforce_essential(E_approx) -% ENFORCE_ESSENTIAL Apply the enforce on E - -% Apply the SVD on E approximate -[U,~,V] = svd( E_approx ); - -% if det(U*V') < 0 -% V = -V; -% end - -% Check the rank of F -rank_E_approx = rank(E_approx); - -% Display the rank -% disp(['Rank of E_approx: ', num2str(rank_E_approx)]); - -info("Rank of E_approx: %.2f \n: ", 2, rank_E_approx); - - -% Creates a valid essential matrix from an approximate solution - -E = U * diag ([1 1 0])* V'; - -% Check the rank of F -rank_E = rank(E); - -% Display the rank -% disp(['Rank of E: ', num2str(rank_E)]); -info("Rank of E: %.2f \n: ", 2, rank_E); - - -end - -%% -function distances = compute_epipolar_errors(F, x1_h_n, x2_h_n) - % Compute epipolar errors for a given fundamental matrix F and point correspondences - % - % Inputs: - % F - Fundamental matrix (3x3) - % x1_h_n - Points in the first image (3xN, homogeneous coordinates) and - % normalized - % x2_h_n - Points in the second image (3xN, homogeneous coordinates) and - % normalized - % - % Output: - % distances - Vector of epipolar distances for each point correspondence - % Compute epipolar lines for points in the second image - l = F * x1_h_n; % Epipolar lines in the second image - - % Normalize the epipolar lines to have unit norm - l = l ./ sqrt(l(1, :).^2 + l(2, :).^2); - - % Compute distances from points in the second image to their epipolar lines - distances = abs(sum(l .* x2_h_n, 1)); - -end -%% - -function [X_best, P] = Cheirality_triangulate(x1_h_n_in, x2_h_n_in, E_ransac) -% CHEIRALITY_TRIANGULATE - Triangulate 3D points and resolve E ambiguity -% Inputs: -% x1, x2 - 2D unnormalized homogeneous points in images 1 and 2 (3xN) -% inliers - Indices of inlier matches after RANSAC -% K - Intrinsic camera matrix (3x3) -% E_ransac - Estimated essential matrix (3x3) -% Outputs: -% X_best - Triangulated 3D points (4xN homogeneous coordinates) -% P - Correct camera projection matrices {P1, P2} - - % Use the selected inlier points for triangulation - - % Initialize best parameters - P2_correct_ind = 0; % Index for correct P2 - max_points_1 = 0; % Maximum valid points satisfying chirality - X_best = []; % Store the best 3D points - - % Step 2: Define camera matrices - % First camera at the origin (canonical form) - P1 = [eye(3), zeros(3, 1)]; - - % Step 3: Decompose Essential Matrix (E) to get R, t candidates - [U, ~, V] = svd(E_ransac); - W = [0 -1 0; 1 0 0; 0 0 1]; % Pre-defined rotation matrix - - % Four possible solutions for [R, t] - R1 = U * W * V'; - R2 = U * W.'*V'; - - t = U(:, 3); - - % Ensure proper rotations (determinant = +1) - if det(R1) < 0, R1 = -R1; end - if det(R2) < 0, R2 = -R2; end - - % Step 4: Generate P2 candidates (Four cases) - P2_candidates = { - [R1, t], - [R1, -t], - [R2, t], - [R2, -t] - }; - - % Step 5: Evaluate each P2 candidate based on chirality condition - for i = 1:4 - P2 = P2_candidates{i}; - - % Triangulate 3D points using current P2 - X = triangulate_3D_point_DLT(x1_h_n_in, x2_h_n_in, P1, P2); - - % Project back to both views - x1_proj = P1 * X; - x2_proj = P2 * X; - - % Count points in front of both cameras (chirality check) - valid_points = (x1_proj(3,:) > 0) & (x2_proj(3,:) > 0); - num_valid = sum(valid_points); - - - % Keep the candidate with the most valid points - if num_valid > max_points_1 - max_points_1 = num_valid; - P2_correct_ind = i; - X_best = X; % (:, valid_points); % Store the best 3D points % Filter valid 3D points - end - end - - - % Step 6: Final output - Correct projection matrices - P{1} = P1; - P{2} = P2_candidates{P2_correct_ind}; - - fprintf('\n--- Cheirality_triangulate Validation Start ---\n'); - - % Print the selected solution - fprintf('Selected P2 Index: %d\n', P2_correct_ind); - fprintf('Number of Valid Points: %d out of %d\n', max_points_1, length(x1_h_n_in)); - - % Optional: Compute reprojection error - err1 = compute_reprojection_error_P(P{1}, X_best, x1_h_n_in); % (:, valid_points)); - err2 = compute_reprojection_error_P(P{2}, X_best, x2_h_n_in); % (:, valid_points)); - fprintf('Mean Reprojection Error: %.4f (Image 1), %.4f (Image 2)\n', mean(err1), mean(err2)); - - fprintf('\n--- Cheirality_triangulate Validation End ---\n'); -end - -%% -function X = triangulate_3D_point_DLT(x1, x2, P1, P2) - % INPUT: - % x1 - 2D points in the first image (3xN homogeneous coordinates) - % x2 - 2D points in the second image (3xN homogeneous coordinates) - % P1 - Camera projection matrix for the first image (3x4) - % P2 - Camera projection matrix for the second image (3x4) - % OUTPUT: - % X - Triangulated 3D points (4xN homogeneous coordinates) - - % Number of points given in x1 and x2 - N = size(x1, 2); - - X = zeros(4, N); - - % Loop through all points to get all 3D points - for i = 1:N - % Construct the A matrix for triangulation - A = [ - x1(1,i) * P1(3,:) - P1(1,:); - x1(2,i) * P1(3,:) - P1(2,:); - - x2(1,i) * P2(3,:) - P2(1,:); - x2(2,i) * P2(3,:) - P2(2,:); - ]; - - % Solve the homogeneous system using SVD as before and we need only - % V to extract v - - [~, ~, V] = svd(A); - X(:,i) = V(:,end); % Last column of V gives the solution ( last eigenvector) - end - - fprintf('\n--- triangulate_3D_point_DLT Validation Start ---\n'); - % Optional: Compute reprojection error - err1 = compute_reprojection_error_P(P1, X, x1); - err2 = compute_reprojection_error_P(P2, X, x2); - fprintf('Mean Reprojection Error: %.4f (Image 1), %.4f (Image 2)\n', mean(err1), mean(err2)); - - fprintf('\n--- triangulate_3D_point_DLT Validation End ---\n'); - - % Normalize the points to make them homogeneous (4th coordinate = 1) - X = X(1:4, :) ./ X(4, :); -end - -%% - -% Function: Robust Translation Estimation -function T_best = estimate_T_robust(xs, Xs, R, inlier_threshold) - - - % Initialization - max_inliers = 0; - N = size(xs, 2); - iterations = 10000; % RANSAC iterations - - for i = 1:iterations - % Sample minimal points (2 points for translation) - sample_indices = randperm(N, 2); - xs_sample = xs(:, sample_indices); - Xs_sample = Xs(:, sample_indices); - - % Estimate candidate T using DLT - T_candidate = estimate_translation_DLT(xs_sample, Xs_sample, R); - - %T_candidate = compute_translation(xs_sample, Xs_sample, R); - - % Compute reprojection errors - % % errors = compute_reprojection_error_P([R,T_candidate], Xs, xs); - errors = compute_reprojection_errors(xs, Xs, R, T_candidate); - - % Count inliers - inliers_candidate = errors < inlier_threshold; - num_inliers = sum(inliers_candidate); - - % Update best model - if num_inliers > max_inliers - max_inliers = num_inliers; - T_best = T_candidate; - inliers = inliers_candidate; - end - end - - fprintf('Percentage of inliers in T_robust: %.2f%%\n', 100 * (max_inliers) / N); - - - % Refine with inliers - if max_inliers == 0 - warning('No inliers found. Returning initial translation.'); - T_best = zeros(3,1); - end - - errors = compute_reprojection_errors(xs, Xs, R, T_best); - - % Count inliers - inliers_candidate = errors.^2 < inlier_threshold.^2; - num_inliers = sum(inliers_candidate); - - fprintf('Percentage of inliers in T_robust best: %.2f%%\n', 100 * (num_inliers) / N); - -end - - - - -function T = estimate_translation_DLT(xs, Xs, R) - % Inputs: - % xs - 2D points in homogeneous coordinates (3xN) - % Xs - 3D points in homogeneous coordinates (4xN) - % R - Rotation matrix (3x3) - - % Extract the first two points - x1 = xs(:,1); - x2 = xs(:,2); - X1 = Xs(:,1); - X2 = Xs(:,2); - - % Construct matrix directly using skew-symmetric form - M = [ - skew(x1) * R * X1(1:3), skew(x1); - skew(x2) * R * X2(1:3), skew(x2) - ]; - - % Solve using SVD - [~, ~, V] = svd(M); - T = V(2:end, end) ./ V(1,4); % Extract translation vector (last column, rows 4-6) -end - -function S = skew(x) - % Constructs the skew-symmetric matrix for cross product - S = [ 0 -x(3) x(2); - x(3) 0 -x(1); - -x(2) x(1) 0 ]; -end - - -% Function: Compute Reprojection Errors -function errors = compute_reprojection_errors(xs, Xs, R, T) - % Project 3D points - projected = (R * Xs(1:3, :) + T); - projected = projected(1:2, :) ./ projected(3, :); % Normalize - - % Compute errors - errors = sqrt(sum((xs(1:2, :) - projected).^2, 1)); % Euclidean distance -end - - -function errors = compute_reprojection_error_P(P, X, x) -% COMPUTE_REPROJECTION_ERROR - Compute reprojection error for 3D points -% Inputs: -% P - Projection matrix (3x4) -% X - 3D points (4xN homogeneous) -% x - 2D points (3xN homogeneous) -% Outputs: -% errors - Reprojection errors (1xN) - - % Project 3D points to 2D - x_proj = P * X; - x_proj = x_proj ./ x_proj(3, :); % Normalize to homogeneous - - % Compute Euclidean distance error - errors = sqrt(sum((x_proj(1:2, :) - x(1:2, :)).^2, 1)); % Pixel errors -end - - -%% LM Helper Functions -function [X_refined, accumulated_error] = LevenbergMarquardt(P1, P2, X, x1, x2, mu, max_iterations, tolerance) - % Levenberg-Marquardt optimization for 3D point refinement - - % Temporary variable for updates - X_temp = X; - accumulated_error = zeros(1, max_iterations); % For plotting error - - for iter = 1:max_iterations - % disp(['Iteration Index: ', num2str(iter)]); - - % Compute total error before update - total_error_before = 0; - for j = 1:size(X_temp, 2) - [err, ~] = ComputeReprojectionError(P1, P2, X_temp(:, j), x1(:, j), x2(:, j)); - total_error_before = total_error_before + err; - end - accumulated_error(iter) = total_error_before; % Store error for plotting - % disp(['Total Error Before: ', num2str(total_error_before)]); - - % Update 3D points - for j = 1:size(X_temp, 2) - % Compute residual and Jacobian for point Xj - [r, J] = LinearizeReprojErr(P1, P2, X_temp(:, j), x1(:, j), x2(:, j)); - - % Compute LM update - delta_Xj = ComputeUpdate(r, J, mu); - - % Update 3D point (temporarily) - X_test = pflat(X_temp(:, j) + delta_Xj); - - % Compute individual errors - [err_before, ~] = ComputeReprojectionError(P1, P2, X_temp(:, j), x1(:, j), x2(:, j)); - [err_after, ~] = ComputeReprojectionError(P1, P2, X_test, x1(:, j), x2(:, j)); - - % Apply update if error improves - if err_after < err_before - X_temp(:, j) = X_test; - end - end - - % Compute total error after update - total_error_after = 0; - for j = 1:size(X_temp, 2) - [err, ~] = ComputeReprojectionError(P1, P2, X_temp(:, j), x1(:, j), x2(:, j)); - total_error_after = total_error_after + err; - end - % disp(['Total Error After: ', num2str(total_error_after)]); - - % Adjust damping factor and check for convergence - if total_error_after < total_error_before - mu = mu / 2; % Decrease damping factor - else - mu = mu * 2; % Increase damping factor - end - - if abs(total_error_after - total_error_before) < tolerance - % disp('Converged!'); - break; - end - end - - % Return refined points and accumulated errors - X_refined = X_temp; -end - - -function J = ComputeJacobian(P, Z) - % Decompose rows of the camera P - P1 = P(1, :); - P2 = P(2, :); - P3 = P(3, :); - - % Compute Jacobian (2x4) - J = [ - (Z(1) * P3 - Z(3) * P1 ) / Z(3)^2; - (Z(2) * P3 - Z(3) * P2 ) / Z(3)^2; - ]; -end - - -function [err, res] = ComputeReprojectionError(P1, P2, Xj, x1j, x2j) - % Project 3D point into both cameras - Z1 = P1 * Xj; - Z2 = P2 * Xj; - - % Compute residuals - r1 = x1j(1:2) - Z1(1:2) ./ Z1(3); % 2x1 - r2 = x2j(1:2) - Z2(1:2) ./ Z2(3); % 2x1 - res = [r1; r2]; % 4x1 - - % Compute total reprojection error - err = sum(res.^2); % 1x1 -end - - -%%% Confirmed - - -function total_error = ComputeTotalError(P1, P2, X, x1, x2) - % Compute the total reprojection error for all 3D points - % - % Inputs: - % - P1, P2: Camera projection matrices - % - X: 4xN matrix of 3D points in homogeneous coordinates - % - x1, x2: 3xN matrices of 2D image points in homogeneous coordinates - % - % Outputs: - % - total_error: The total reprojection error across all points - - total_error = 0; % Initialize total error - for j = 1:size(X, 2) - [err, ~] = ComputeReprojectionError(P1, P2, X(:, j), x1(:, j), x2(:, j)); - total_error = total_error + err; - end -end - -function delta_Xj = ComputeUpdate(r, J, mu) - % Compute normal equations for the update - H = J.' * J + mu * eye(size(J, 2)); % Hessian approximation (4x4) - g = J.' * r; % Gradient (4x1) - - % Compute the update step - delta_Xj = -H \ g; % inv(H) -% Computes the LM update . -end - - -function [r, J] = LinearizeReprojErr(P1, P2, Xj, x1j, x2j) - % Project 3D point into both cameras - Z1 = P1 * Xj; - Z2 = P2 * Xj; - - % Compute residuals - r1 = x1j(1:2) - Z1(1:2) ./ Z1(3); % 2x1 - r2 = x2j(1:2) - Z2(1:2) ./ Z2(3); % 2x1 - r = [r1; r2]; % 4x1 - - % Compute Jacobians using chain rule - J1 = ComputeJacobian(P1, Z1); % 2x4 - J2 = ComputeJacobian(P2, Z2); % 2x4 - - % Combine Jacobians - J = [J1; J2]; % 4x4 -end - - - - -%% Other helper functions - - -function [V] = pflat(V) -% Normalize the n points in P^(m-1) - -V = V(:,:) ./ V(end, :); - -end -%% -function plotcams(P) - % Function to plot camera positions and viewing directions - % Inputs: - % P - Cell array of camera projection matrices - - % Initialize arrays to store camera centers and directions - c = zeros(4, length(P)); % Camera centers - v = zeros(3, length(P)); % Viewing directions - - for i = 1:length(P) - c(:,i) = null(P{i}); % Compute camera center (homogeneous coordinates) - v(:,i) = P{i}(3, 1:3); % Viewing direction (3rd row of P) - end - - % Normalize homogeneous coordinates - c = c ./ repmat(c(4,:), [4 1]); % Convert to 3D by dividing by the last element - - % Plot cameras using quiver3 - quiver3(c(1,:), c(2,:), c(3,:), v(1,:), v(2,:), v(3,:), 'r-'); - hold on; - - - % %Label all cameras - % for i = 1:length(P) - % label = sprintf('Camera %d', i); - % text(c(1,i), c(2,i), c(3,i), label, 'FontSize', 1, 'Color', 'b'); - % end - - % Enhance the plot for clarity - xlabel('X'); - ylabel('Y'); - zlabel('Z'); - grid on; - axis equal; - hold off; -end -%% - -function v_h = toHomogeneous(v) -% TOHOMOGENEOUS - Converts a vector to homogeneous coordinates. -% -% Usage: -% v_h = toHomogeneous(v) -% -% Inputs: -% v - Input vector (NxM) where N is the dimensionality, and M is the number of points. -% -% Outputs: -% v_h - Output vector in homogeneous coordinates ((N+1)xM). -% -% Example: -% v = [1; 2; 3]; % 3D point -% v_h = toHomogeneous(v); % Convert to homogeneous -> [1; 2; 3; 1] - -% Validate input -if nargin < 1 - error('Input vector is required.'); -end - -% Add a row of ones to make it homogeneous -v_h = [v; ones(1, size(v, 2))]; - -end - -%% -function validate_E(E) -% VALIDATE_E - Validates the Essential matrix E based on rank, singular values, -% epipolar constraint, and determinant. -% -% Inputs: -% E - Essential matrix (3x3) -% x1 - Normalized homogeneous coordinates of points in image 1 (3xN) -% x2 - Normalized homogeneous coordinates of points in image 2 (3xN) -% -% Example: -% validate_E(E_candidate, x1_h_n_sample, x2_h_n_sample); - - % fprintf('\n--- Essential Matrix Validation Start ---\n'); - info("\n--- Essential Matrix Validation Start ---\n", 2); - - % 1. Rank Constraint - rank_E = rank(E); % Compute rank - % fprintf('Rank of E: %d (Expected: 2)\n', rank_E); - - info("Rank of E: %d (Expected: 2)\n: ", 2, rank_E); - - - - % 2. Singular Value Constraint - [U, S, V] = svd(E); % SVD - singular_values = diag(S); - % fprintf('Singular values of E: [%.6f, %.6f, %.6f]\n', singular_values); - - info("Singular values of E: [%.6f, %.6f, %.6f]\n", 2, singular_values); - - - if abs(S(1,1) - S(2,2)) < 1e-6 && S(3,3) < 1e-6 - % fprintf('Singular value constraint: Satisfied\n'); - info("Singular value constraint: Satisfied\n", 2); - - else - % fprintf('Singular value constraint: Not satisfied\n'); - info("Singular value constraint: Not satisfied\n", 2); - - end - - % 4. Determinant Check - det_E = det(E); % Compute determinant - % fprintf('Determinant of E: %.6f (Expected: Close to 0)\n', det_E); - info("Determinant of E: %.6f (Expected: Close to 0)\n", 2, det_E); - - - % Final Assessment - if rank_E == 2 && abs(S(1,1) - S(2,2)) < 1e-6 && S(3,3) < 1e-6 && abs(det_E) < 1e-6 - % fprintf('Validation Status: PASS - Essential matrix is valid.\n'); - info("Validation Status: PASS - Essential matrix is valid.\n", 2); - - else - % fprintf('Validation Status: FAIL - Essential matrix is invalid.\n'); - info("Validation Status: FAIL - Essential matrix is invalid.\n", 2); - end - - % fprintf('\n--- Essential Matrix Validation End ---\n'); - info("\n--- Essential Matrix Validation End ---\n", 2); - -end - -%% -function validate_camera(P) -% VALIDATE_CAMERA - Validates the projection matrix of a camera -% -% Inputs: -% P - 3x4 Camera projection matrix -% K - 3x3 Intrinsic parameter camera matrix -% Outputs: -% Prints validation results for R and T: -% - Orthogonality error of R -% - Determinant of R -% - Translation vector (T) and its norm - - fprintf('\n--- validate_camera Validation Start ---\n'); - - - % --- Extract Rotation (R) and Translation (T) --- - R = P(1:3, 1:3); % Rotation matrix - T = P(1:3, 4); % Translation vector - - % --- Validate Orthogonality of R --- - orthogonality_error = norm(R * R' - eye(3)); % Should be close to 0 - fprintf('R Orthogonality Error: %.6e\n', orthogonality_error); - - % --- Validate Determinant of R --- - det_R = det(R); % Should be close to +1 - fprintf('Determinant of R: %.6f\n', det_R); - - % --- Display Translation Information --- - fprintf('Translation Vector T: [%.4f, %.4f, %.4f]\n', T(1), T(2), T(3)); - fprintf('Norm of T: %.4f\n', norm(T)); - - % --- Additional Checks --- - % Warn if errors exceed acceptable thresholds - if orthogonality_error > 1e-6 - warning('R is not orthogonal. Check rotation matrix!'); - end - if abs(det_R - 1) > 1e-3 - warning('Determinant of R is not 1. Check rotation matrix!'); - end - - fprintf('\n--- validate_camera Validation End ---\n'); - -end - -%% - -function info(message, varargin) -% INFO - Custom function to display hierarchical information messages. -% Prints messages based on the global info_level and enableInfo settings. -% -% Usage: -% info('Message') % Defaults to Level 1 -% info('Message', 1) % Explicit Level 1 -% info('Message with %d', 1, 10) % Level 1 with variables -% info('Message with %f', 2, 3.14159) % Level 2 with variables -% -% Global Settings: -% - enableInfo: Toggle info messages (true/false). -% - info_level: Controls verbosity (1 = only Level 1, 2 = Level 1 and 2). - -% Access global variables -global enableInfo; -global info_level; - -% Set default level to 1 if not explicitly provided -if nargin < 2 || ~isnumeric(varargin{1}) % Check if level is missing - level = 1; % Default level - args = varargin; % Treat everything else as variables -else - level = varargin{1}; % Extract level - args = varargin(2:end); % Remaining are variables -end - -% Print messages based on levels -if enableInfo - % Print if the current level is less than or equal to the global level - if level <= info_level - fprintf('[INFO][Level %d]: ', level); - fprintf(message, args{:}); % Pass additional arguments - fprintf('\n'); - end -end -end - -%% - -function X_front= points_in_front(X_points) - % remove points behind the camera - X_front = X_points(:,X_points(3,:)>0); -end - - -function X_good = keep_good_points(X_front) - % remove points far away from center of gravity - center_points = mean(X_front(1:3,:), 2); - distance_from_center = sqrt(sum((X_front(1:3,:) - center_points).^2, 1)); - threshold = 4 * quantile(distance_from_center, 0.9); - ind_clean = distance_from_center < threshold; - X_good = X_front(:, ind_clean); -end - -%% -function P_un = CalibratedToUncalibrated(P_ca, K) - -N = length(P_ca); - -P_un = cell(1,N); -for i=1:N - P_un{i} = K * P_ca{i}; -end +info("The SFM Algorithm is Finished"); end \ No newline at end of file diff --git a/toHomogeneous.m b/toHomogeneous.m new file mode 100644 index 0000000..b70a681 --- /dev/null +++ b/toHomogeneous.m @@ -0,0 +1,25 @@ +function v_h = toHomogeneous(v) +% TOHOMOGENEOUS - Converts a vector to homogeneous coordinates. +% +% Usage: +% v_h = toHomogeneous(v) +% +% Inputs: +% v - Input vector (NxM) where N is the dimensionality, and M is the number of points. +% +% Outputs: +% v_h - Output vector in homogeneous coordinates ((N+1)xM). +% +% Example: +% v = [1; 2; 3]; % 3D point +% v_h = toHomogeneous(v); % Convert to homogeneous -> [1; 2; 3; 1] + +% Validate input +if nargin < 1 + error('Input vector is required.'); +end + +% Add a row of ones to make it homogeneous +v_h = [v; ones(1, size(v, 2))]; + +end diff --git a/Helper functions/triangulate_3D_point_DLT.m b/triangulate_3D_point_DLT.m similarity index 73% rename from Helper functions/triangulate_3D_point_DLT.m rename to triangulate_3D_point_DLT.m index 46eca51..269f189 100644 --- a/Helper functions/triangulate_3D_point_DLT.m +++ b/triangulate_3D_point_DLT.m @@ -1,35 +1,44 @@ -function X = triangulate_3D_point_DLT(x1, x2, P1, P2) - % INPUT: - % x1 - 2D points in the first image (3xN homogeneous coordinates) - % x2 - 2D points in the second image (3xN homogeneous coordinates) - % P1 - Camera projection matrix for the first image (3x4) - % P2 - Camera projection matrix for the second image (3x4) - % OUTPUT: - % X - Triangulated 3D points (4xN homogeneous coordinates) - - % Number of points given in x1 and x2 - N = size(x1, 2); - - X = zeros(4, N); - - % Loop through all points to get all 3D points - for i = 1:N - % Construct the A matrix for triangulation - A = [ - x1(1,i) * P1(3,:) - P1(1,:); - x1(2,i) * P1(3,:) - P1(2,:); - - x2(1,i) * P2(3,:) - P2(1,:); - x2(2,i) * P2(3,:) - P2(2,:); - ]; - - % Solve the homogeneous system using SVD as before and we need only - % V to extract v - - [~, ~, V] = svd(A); - X(:,i) = V(:,end); % Last column of V gives the solution ( last eigenvector) - end - - % Normalize the points to make them homogeneous (4th coordinate = 1) - X = X(1:4, :) ./ X(4, :); +function X = triangulate_3D_point_DLT(x1, x2, P1, P2) + % INPUT: + % x1 - 2D points in the first image (3xN homogeneous coordinates) + % x2 - 2D points in the second image (3xN homogeneous coordinates) + % P1 - Camera projection matrix for the first image (3x4) + % P2 - Camera projection matrix for the second image (3x4) + % OUTPUT: + % X - Triangulated 3D points (4xN homogeneous coordinates) + + % Number of points given in x1 and x2 + N = size(x1, 2); + + X = zeros(4, N); + + % Loop through all points to get all 3D points + for i = 1:N + % Construct the A matrix for triangulation + A = [ + x1(1,i) * P1(3,:) - P1(1,:); + x1(2,i) * P1(3,:) - P1(2,:); + + x2(1,i) * P2(3,:) - P2(1,:); + x2(2,i) * P2(3,:) - P2(2,:); + ]; + + % Solve the homogeneous system using SVD as before and we need only + % V to extract v + + [~, ~, V] = svd(A); + X(:,i) = V(:,end); % Last column of V gives the solution ( last eigenvector) + end + + info("\n--- triangulate_3D_point_DLT Validation Start ---\n"); + % Optional: Compute reprojection error + err1 = compute_reprojection_error_P(P1, X, x1); + err2 = compute_reprojection_error_P(P2, X, x2); + + info('Mean Reprojection Error: %.4f (Image 1), %.4f (Image 2)\n', 1, mean(err1), mean(err2)); + + info('\n--- triangulate_3D_point_DLT Validation End ---\n'); + + % Normalize the points to make them homogeneous (4th coordinate = 1) + X = X(1:4, :) ./ X(4, :); end \ No newline at end of file diff --git a/validate_E.m b/validate_E.m new file mode 100644 index 0000000..d512aa4 --- /dev/null +++ b/validate_E.m @@ -0,0 +1,61 @@ +function validate_E(E) +% VALIDATE_E - Validates the Essential matrix E based on rank, singular values, +% epipolar constraint, and determinant. +% +% Inputs: +% E - Essential matrix (3x3) +% x1 - Normalized homogeneous coordinates of points in image 1 (3xN) +% x2 - Normalized homogeneous coordinates of points in image 2 (3xN) +% +% Example: +% validate_E(E_candidate, x1_h_n_sample, x2_h_n_sample); + + % fprintf('\n--- Essential Matrix Validation Start ---\n'); + info("\n--- Essential Matrix Validation Start ---\n", 2); + + % 1. Rank Constraint + rank_E = rank(E); % Compute rank + % fprintf('Rank of E: %d (Expected: 2)\n', rank_E); + + info("Rank of E: %d (Expected: 2)\n: ", 2, rank_E); + + + + % 2. Singular Value Constraint + [U, S, V] = svd(E); % SVD + singular_values = diag(S); + % fprintf('Singular values of E: [%.6f, %.6f, %.6f]\n', singular_values); + + info("Singular values of E: [%.6f, %.6f, %.6f]\n", 2, singular_values); + + + if abs(S(1,1) - S(2,2)) < 1e-6 && S(3,3) < 1e-6 + % fprintf('Singular value constraint: Satisfied\n'); + info("Singular value constraint: Satisfied\n", 2); + + else + % fprintf('Singular value constraint: Not satisfied\n'); + info("Singular value constraint: Not satisfied\n", 2); + + end + + % 4. Determinant Check + det_E = det(E); % Compute determinant + % fprintf('Determinant of E: %.6f (Expected: Close to 0)\n', det_E); + info("Determinant of E: %.6f (Expected: Close to 0)\n", 2, det_E); + + + % Final Assessment + if rank_E == 2 && abs(S(1,1) - S(2,2)) < 1e-6 && S(3,3) < 1e-6 && abs(det_E) < 1e-6 + % fprintf('Validation Status: PASS - Essential matrix is valid.\n'); + info("Validation Status: PASS - Essential matrix is valid.\n", 2); + + else + % fprintf('Validation Status: FAIL - Essential matrix is invalid.\n'); + info("Validation Status: FAIL - Essential matrix is invalid.\n", 2); + end + + % fprintf('\n--- Essential Matrix Validation End ---\n'); + info("\n--- Essential Matrix Validation End ---\n", 2); + +end diff --git a/validate_camera.m b/validate_camera.m new file mode 100644 index 0000000..c93f484 --- /dev/null +++ b/validate_camera.m @@ -0,0 +1,45 @@ +function validate_camera(P) +% VALIDATE_CAMERA - Validates the projection matrix of a camera +% +% Inputs: +% P - 3x4 Camera projection matrix +% K - 3x3 Intrinsic parameter camera matrix +% Outputs: +% Prints validation results for R and T: +% - Orthogonality error of R +% - Determinant of R +% - Translation vector (T) and its norm + + info("\n--- validate_camera Validation Start ---\n",2); + + + % --- Extract Rotation (R) and Translation (T) --- + R = P(1:3, 1:3); % Rotation matrix + T = P(1:3, 4); % Translation vector + + % --- Validate Orthogonality of R --- + orthogonality_error = norm(R * R' - eye(3)); % Should be close to 0 + + info("R Orthogonality Error: %.6e\n",2,orthogonality_error); + + + % --- Validate Determinant of R --- + det_R = det(R); % Should be close to +1 + info("Determinant of R: %.6f\n",2, det_R); + + % --- Display Translation Information --- + info("Translation Vector T: [%.4f, %.4f, %.4f]\n",2, T(1), T(2), T(3)); + info("Norm of T: %.4f\n",2, norm(T)); + + % --- Additional Checks --- + % Warn if errors exceed acceptable thresholds + if orthogonality_error > 1e-6 + warning('R is not orthogonal. Check rotation matrix!'); + end + if abs(det_R - 1) > 1e-3 + warning('Determinant of R is not 1. Check rotation matrix!'); + end + + info("\n--- validate_camera Validation End ---\n",2, norm(T)); + +end