How to find the orientation of the entire platform by looking at the overall geometey of the objects/dots?

Hi, Im trying to find the orientation that is roll and pitch angle of my platform by analysing the overall geometery of the dots. To do so i have firstly detected the dots by using standard deviation and k-means clustering and dividing the image into 5x5 boxes then a plane fitting curve method is used to determine the overall geometery of the dots which further gives orientation of the plate. The obtained angles are incorrect what might have possibly gone wrong please help and suggest any possible solution. I'm actually trying to implement a blass on a plate system and i want to detect the orientation of the plate using webcam. Also im using MATLAB 2021a therefore im not sure how to calibrate my camera for such a pattern. The MATLAB code is given below;

clear all;

clc;

close all;

% Create a webcam object

cap = webcam('Adesso CyberTrack H3');

% Adjust camera settings if supported

try

% Set the exposure time (shutter speed) in seconds

exposureTime = 0.01; % Adjust as needed

cap.ExposureMode = 'manual';

cap.Exposure = exposureTime;

catch ME

disp('Warning: Unable to set exposure time for the camera.');

disp(ME.message);

end

% Create the background subtractor object for ball detection

back_sub = vision.ForegroundDetector('NumTrainingFrames', 100, 'NumGaussians', 3, 'MinimumBackgroundRatio', 0.7);

% Create structuring element for morphological operation

se = strel('rectangle', [20, 20]);

% Initialize calibration values

calibration_pitch = 0;

calibration_roll = 0;

% Calibrate the system to set initial pitch and roll to zero

frame = snapshot(cap);

I_gray = rgb2gray(frame);

I_contrast = imadjust(I_gray);

bw = imbinarize(I_contrast, 'adaptive');

bw_filled = imfill(bw, 'holes');

bw_cleaned = bwareaopen(bw_filled, 50);

stats = regionprops('table', bw_cleaned, 'Centroid', 'BoundingBox');

centroids = stats.Centroid;

if size(centroids, 1) >= 3

X = centroids(:, 1);

Y = centroids(:, 2);

Z = linspace(0, 1, size(centroids, 1))';

A = [X, Y, ones(size(X))];

coeffs = A \ Z;

normal = [-coeffs(1), -coeffs(2), 1];

normal = normal / norm(normal);

calibration_pitch = asin(normal(1));

calibration_roll = asin(normal(2));

disp(['Calibration Pitch: ', num2str(calibration_pitch), ' degrees']);

disp(['Calibration Roll: ', num2str(calibration_roll), ' degrees']);

else

disp('Not enough dots detected for calibration.');

end

while true

% Read frame from webcam

frame = snapshot(cap);

% Part 1: Dot Detection and Tilt Angle Calculation

I_gray = rgb2gray(frame);

I_contrast = imadjust(I_gray);

bw = imbinarize(I_contrast, 'adaptive');

bw_filled = imfill(bw, 'holes');

bw_cleaned = bwareaopen(bw_filled, 50);

stats = regionprops('table', bw_cleaned, 'Centroid', 'BoundingBox');

centroids = stats.Centroid;

boundingBoxes = stats.BoundingBox;

if size(centroids, 1) >= 3

X = centroids(:, 1);

Y = centroids(:, 2);

Z = linspace(0, 1, size(centroids, 1))';

A = [X, Y, ones(size(X))];

coeffs = A \ Z;

normal = [-coeffs(1), -coeffs(2), 1];

normal = normal / norm(normal);

pitch = asin(normal(1)) - calibration_pitch;

roll = asin(normal(2)) - calibration_roll;

yaw = 0;

disp(['Pitch (Tilt around Y-axis): ', num2str(pitch), ' degrees']);

disp(['Roll (Tilt around X-axis): ', num2str(roll), ' degrees']);

disp(['Yaw (Rotation around Z-axis): ', num2str(yaw), ' degrees']);

else

pitch = 0;

roll = 0;

yaw = 0;

disp('Not enough dots detected to fit a plane.');

end

for i = 1:size(boundingBoxes, 1)

frame = insertShape(frame, 'Rectangle', boundingBoxes(i, :), 'Color', 'g', 'LineWidth', 2);

end

fg_mask = step(back_sub, frame);

fg_mask = imclose(fg_mask, se);

fg_mask = medfilt2(fg_mask, [5, 5]);

fg_mask = uint8(fg_mask) * 255;

[contours, ~] = bwlabel(fg_mask);

stats = regionprops(contours, 'Area', 'BoundingBox', 'Centroid');

if ~isempty(stats)

[~, max_index] = max([stats.Area]);

centroid = stats(max_index).Centroid;

boundingBox = stats(max_index).BoundingBox;

frame = insertShape(frame, 'Rectangle', boundingBox, 'Color', [0, 255, 0], 'LineWidth', 3);

text = ['x: ', num2str(centroid(1)), ', y: ', num2str(centroid(2))];

frame = insertText(frame, [centroid(1) - 10, centroid(2) - 10], text, 'FontSize', 50, 'TextColor', [0, 255, 0]);

coordinates = [];

coordinates = [coordinates; centroid];

disp(coordinates);

end

frame = insertText(frame, [10, 10], ['Pitch: ', num2str(pitch), '°'], 'FontSize', 18, 'BoxColor', 'red', 'TextColor', 'white');

frame = insertText(frame, [10, 40], ['Roll: ', num2str(roll), '°'], 'FontSize', 18, 'BoxColor', 'red', 'TextColor', 'white');

frame = insertText(frame, [10, 70], ['Yaw: ', num2str(yaw), '°'], 'FontSize', 18, 'BoxColor', 'red', 'TextColor', 'white');

imshow(frame);

pause(0.0001);

end

clear cap;

Answer 1

[Kaustab Pal]

Hi @shaherbano zaidi,

There might be an error in the computation of the pitch and roll angles in your code.

The pitch angle is the angle between the normal vector's projection onto the X-Z plane and the Z-axis. It can be calculated as:

pitch = atan2(normal(1), normal(3));

Similarly, the roll angle is the angle between the normal vector's projection onto the Y-Z plane and the Z-axis. It can be calculated as:

roll = atan2(normal(2), normal(3));

Correcting these calculations should provide you with accurate results.

Additionally, to fit a precise plane, consider using RANSAC, which is more robust to outliers by iteratively selecting random subsets of data and fitting a model to these subsets.. Calibrating your camera will also significantly improve the accuracy of your results.

You can find the documentation for RANSAC here: https://www.mathworks.com/help/releases/R2021a/vision/ref/ransac.html

For Camera Calibration documentation, refer to this link: https://www.mathworks.com/help/releases/R2021a/vision/camera-calibration.html

Hope this helps.

Best regards,

Kaustab