Code is getting struck

Hi all! Below is my code where blue,red and green colored markers are tracked , a triangle is formed and their vertices are obtained. This is for verification of my camera calibration and another application. But I am not able to obtain the correct result and my code is getting struck at around 21'st video frame. please help!

clear all; %display values of cent's..run rgbplot2D.m
imaqreset;
vid = videoinput('winvideo',1,'MJPG_800x480');
vid2 = videoinput('winvideo',2,'MJPG_800x480');
set(vid,'FramesPerTrigger',Inf);
set(vid, 'ReturnedColorSpace','rgb');
vid.FrameGrabInterval =5;
set(vid2,'FramesPerTrigger',Inf);
set(vid2, 'ReturnedColorSpace','rgb');
vid2.FrameGrabInterval =5;
k = 0; c=0; h=0; tic;
%%cent =zeros(10,2); co= 0;
cent=[0,0];
centg=[0,0];
centg2=[0,0];
cent2= [0,0];
centb=[0,0];
centb2=[0,0];
start(vid);
start(vid2);
preview(vid);
preview(vid2);

while ((vid.FramesAcquired<=200) && (vid2.FramesAcquired<=200))
    tstart=tic;

data = getsnapshot(vid);
data2 = getsnapshot(vid2);
%-------------------REDDDDDDDDDDDDD------------%
diff = imsubtract(data(:,:,1),rgb2gray(data));
diff2 = imsubtract(data2(:,:,1),rgb2gray(data2));
diff = im2bw(diff, 0.18);
diff2 = im2bw(diff2, 0.18);
diff = bwareaopen(diff , 300);
diff2 = bwareaopen(diff2 , 300);
bw = bwlabel(diff , 8);
bw2 = bwlabel(diff2 , 8);
stats = regionprops(bw, 'BoundingBox', 'Centroid');
stats2 = regionprops(bw2, 'BoundingBox', 'Centroid');

%-------------------GREEENNNNNNN-------------%
diffgreen = imsubtract(data(:,:,2),rgb2gray(data));
diffgreen2 = imsubtract(data2(:,:,2),rgb2gray(data2));
diffgreen = im2bw(diffgreen, 0.02);
diffgreen2 = im2bw(diffgreen2, 0.02);
diffgreen = bwareaopen(diffgreen , 300);
diffgreen2 = bwareaopen(diffgreen2 , 300);
bwgreen = bwlabel(diffgreen , 8);
bwgreen2 = bwlabel(diffgreen2 , 8);
statsgreen = regionprops(bwgreen, 'BoundingBox', 'Centroid');
statsgreen2 = regionprops(bwgreen2, 'BoundingBox', 'Centroid');

%---------------------BBLLLUUEEEEEE----------%

diffblue = imsubtract(data(:,:,3),rgb2gray(data));
diffblue2 = imsubtract(data2(:,:,3),rgb2gray(data2));
diffblue = im2bw(diffblue, 0.15);
diffblue2 = im2bw(diffblue2, 0.15);
diffblue = bwareaopen(diffblue , 300);
diffblue2 = bwareaopen(diffblue2 , 300);
bwblue = bwlabel(diffblue , 8);
bwblue2 = bwlabel(diffblue2 , 8);
statsblue = regionprops(bwblue, 'BoundingBox', 'Centroid');
statsblue2 = regionprops(bwblue2, 'BoundingBox', 'Centroid');

imshow(data2);
hold on
%---------------RED...CAM1 AND CAM2-------------------%
for (object = 1:length(stats))
bb = stats(object).BoundingBox; 
bc = stats(object).Centroid; 
rectangle('Position',bb,'EdgeColor','r','LineWidth',2)
cent(object,:) = stats(object).Centroid;
plot(bc(1),bc(2), '-m+')
a=text(bc(1)+15,bc(2), strcat('Xr1: ', num2str(round(bc(1))), ' Yr1: ', num2str(round(bc(2)))));
set(a, 'FontName', 'Arial', 'FontWeight', 'bold', 'FontSize', 12, 'Color', 'yellow');
end
 for( object2 = 1:length(stats2))
 bb2 = stats2(object2).BoundingBox;
 bc2 = stats2(object2).Centroid;
rectangle('Position',bb2,'EdgeColor','r','LineWidth',2,'LineStyle',':');
cent2(object2,:) = stats2(object2).Centroid;
plot(bc2(1),bc2(2), '-m+')
a2=text(bc2(1)+15,bc2(2), strcat('Xr2:', num2str(round(bc2(1))), ' Yr2: ', num2str(round(bc2(2)))));
set(a2, 'FontName', 'Arial', 'FontWeight', 'bold', 'FontSize', 12, 'Color', 'green');
end

xrCentroids = mean(cent(:,1));
yrCentroids = mean(cent(:,2));
b=text(xrCentroids, yrCentroids , strcat('   X: ', num2str(round(xrCentroids)),'Y: ', num2str(round(yrCentroids))));
set(b, 'FontName', 'Arial', 'FontWeight', 'bold', 'FontSize', 10, 'Color', 'white');
xrCentroids2 = mean(cent2(:,1));
yrCentroids2 = mean(cent2(:,2));
b2=text(xrCentroids2, yrCentroids2 , strcat('X: ', num2str(round(xrCentroids2)),'Y: ', num2str(round(yrCentroids2))));

set(b2, 'FontName', 'Arial', 'FontWeight', 'bold', 'FontSize', 10, 'Color', 'blue');

%-------------GREEN....CAM1 AND CAM2------------------------%
for objectg = 1:length(statsgreen)
bbg = statsgreen(objectg).BoundingBox;
bcg = statsgreen(objectg).Centroid;
rectangle('Position',bbg,'EdgeColor','g','LineWidth',2)
centg(objectg,:) = statsgreen(objectg).Centroid;

plot(bcg(1),bcg(2), '-m+')
g=text(bcg(1)+15,bcg(2), strcat('X: ', num2str(round(bcg(1))), ' Y: ', num2str(round(bcg(2)))));
set(g, 'FontName', 'Arial', 'FontWeight', 'bold', 'FontSize', 12, 'Color', 'yellow');
end
 for( objectg2 = 1:length(statsgreen2))
 bbg2 = statsgreen2(objectg2).BoundingBox;
 bcg2 = statsgreen2(objectg2).Centroid;
rectangle('Position',bbg2,'EdgeColor','g','LineWidth',2,'LineStyle',':');
centg2(objectg2,:) = statsgreen2(objectg2).Centroid;
plot(bcg2(1),bcg2(2), '-m+')
a2=text(bcg2(1)+15,bcg2(2), strcat('Xr2:', num2str(round(bcg2(1))), ' Yr2: ', num2str(round(bcg2(2)))));
set(a2, 'FontName', 'Arial', 'FontWeight', 'bold', 'FontSize', 12, 'Color', 'green');
end

xgCentroids = mean(centg(:,1));
ygCentroids = mean(centg(:,2));
b=text(xgCentroids, ygCentroids , strcat('   Xg1: ', num2str(round(xgCentroids)),'Yg1: ', num2str(round(ygCentroids))));
set(b, 'FontName', 'Arial', 'FontWeight', 'bold', 'FontSize', 10, 'Color', 'white');
xgCentroids2 = mean(centg2(:,1));
ygCentroids2 = mean(centg2(:,2));
b2=text(xgCentroids2, ygCentroids2 , strcat('Xg1: ', num2str(round(xgCentroids2)),'Yg2: ', num2str(round(ygCentroids2))));
set(b2, 'FontName', 'Arial', 'FontWeight', 'bold', 'FontSize', 10, 'Color', 'blue');

%----------------BLUE..........CAM1 AND CAM2--------------------%

for objectb = 1:length(statsblue)
bbb = statsblue(objectb).BoundingBox;
bcb = statsblue(objectb).Centroid;
rectangle('Position',bbb,'EdgeColor','b','LineWidth',2)
centb(objectb,:) = statsblue(objectb).Centroid;

plot(bcb(1),bcb(2), '-m+')
b=text(bcb(1)+15,bcb(2), strcat('Xb1: ', num2str(round(bcb(1))), ' Yb1: ', num2str(round(bcb(2)))));
set(b, 'FontName', 'Arial', 'FontWeight', 'bold', 'FontSize', 12, 'Color', 'yellow');
end
 for( objectb2 = 1:length(statsblue2))
 bbb2 = statsblue2(objectb2).BoundingBox;
 bcb2 = statsblue2(objectb2).Centroid;
rectangle('Position',bbb2,'EdgeColor','b','LineWidth',2,'LineStyle',':');
centb2(objectb2,:) = statsblue2(objectb2).Centroid;
plot(bcb2(1),bcb2(2), '-m+')
a2=text(bcb2(1)+15,bcb2(2), strcat('Xb2:', num2str(round(bcb2(1))), ' Yb2: ', num2str(round(bcb2(2)))));
set(a2, 'FontName', 'Arial', 'FontWeight', 'bold', 'FontSize', 12, 'Color', 'green');
end

xbCentroids = mean(centb(:,1));
ybCentroids = mean(centb(:,2));
b=text(xbCentroids, ybCentroids , strcat('   X: ', num2str(round(xbCentroids)),'Y: ', num2str(round(ybCentroids))));
set(b, 'FontName', 'Arial', 'FontWeight', 'bold', 'FontSize', 10, 'Color', 'white');
xbCentroids2 = mean(centb2(:,1));
ybCentroids2 = mean(centb2(:,2));
b2=text(xbCentroids2, ybCentroids2 , strcat('X: ', num2str(round(xbCentroids2)),'Y: ', num2str(round(ybCentroids2))));
set(b2, 'FontName', 'Arial', 'FontWeight', 'bold', 'FontSize', 10, 'Color', 'blue');
end

and this is for plotting the marked points and finding the vertices.

fprintf('CALCULATING THE VERTICES FOR LEFT CAMERA...\n')
cent = cent';
centg = centg';
centb = centb';
fprintf('VERTICES FOR RED GREEN\n\n\n')
[x1] = cent(1,:);
[y1] = cent(2,:);

          ma = (y1(2)-y1(1))/(x1(2)-x1(1));
          na = y1(1) - ma*x1(1);

       %For Line 2
       [x2] = centg(1,:);
       [y2] = centg(2,:); 

          mb = (y2(2)-y2(1))/(x2(2)-x2(1));
          nb = y2(1) - mb*x2(1);

syms x
x=double(solve(x*ma + na == x*mb + nb))
y=double(x*ma+na)

xL1 =[x;y];

fprintf('VERTICES FOR GREEN BLUE\n')
[x1] = centg(1,:);
[y1] = centg(2,:);

          ma = (y1(2)-y1(1))/(x1(2)-x1(1));
          na = y1(1) - ma*x1(1);

       %For Line 2
       [x2] = centb(1,:);
       [y2] = centb(2,:); 

          mb = (y2(2)-y2(1))/(x2(2)-x2(1));
          nb = y2(1) - mb*x2(1);

syms x
x=double(solve(x*ma + na == x*mb + nb))
y=double(x*ma+na)

xL2 = [x;y];

fprintf('VERTICES FOR RED BLUE\n')
[x1] = cent(1,:);
[y1] = cent(2,:);

          ma = (y1(2)-y1(1))/(x1(2)-x1(1));
          na = y1(1) - ma*x1(1);

       %For Line 2
       [x2] = centb(1,:);
       [y2] = centb(2,:); 

          mb = (y2(2)-y2(1))/(x2(2)-x2(1));
          nb = y2(1) - mb*x2(1);

syms x
x=double(solve(x*ma + na == x*mb + nb))
y=double(x*ma+na)

xL3 = [x;y];

fprintf('\n\nCALCULATING THE VERTICES FOR RIGHT CAMERA...\n')
cent2 = cent2';
centg2 = centg2';
centb2 = centb2';
fprintf('VERTICES FOR RED GREEN\n\n\n')
[x1] = cent2(1,:);
[y1] = cent2(2,:);

          ma = (y1(2)-y1(1))/(x1(2)-x1(1));
          na = y1(1) - ma*x1(1);

       %For Line 2
       [x2] = centg2(1,:);
       [y2] = centg2(2,:); 

          mb = (y2(2)-y2(1))/(x2(2)-x2(1));
          nb = y2(1) - mb*x2(1);

syms x
x=double(solve(x*ma + na == x*mb + nb))
y=double(x*ma+na)

xR1 = [x;y];

fprintf('VERTICES FOR GREEN BLUE\n')
[x1] = centg2(1,:);
[y1] = centg2(2,:);

          ma = (y1(2)-y1(1))/(x1(2)-x1(1));
          na = y1(1) - ma*x1(1);

       %For Line 2
       [x2] = centb2(1,:);
       [y2] = centb2(2,:); 

          mb = (y2(2)-y2(1))/(x2(2)-x2(1));
          nb = y2(1) - mb*x2(1);

syms x
x=double(solve(x*ma + na == x*mb + nb))
y=double(x*ma+na)

xR2 = [x;y];

fprintf('VERTICES FOR RED BLUE\n')
[x1] = cent2(1,:);
[y1] = cent2(2,:);

          ma = (y1(2)-y1(1))/(x1(2)-x1(1));
          na = y1(1) - ma*x1(1);

       %For Line 2
       [x2] = centb2(1,:);
       [y2] = centb2(2,:); 

          mb = (y2(2)-y2(1))/(x2(2)-x2(1));
          nb = y2(1) - mb*x2(1);

syms x
x=double(solve(x*ma + na == x*mb + nb))
y=double(x*ma+na)

xR3 = [x;y];

fprintf('\n\nSTEREO TRIANGULATION\n\n ')
load_stereo_calib_files;
fprintf('3D co-ords of RED GREEN')
A= stereo_triangulation(xL1,xR1,om,T,fc_left,cc_left,kc_left,alpha_c_left,fc_right,cc_right,kc_right,alpha_c_right)

fprintf('3D co-ords of GREEN BLUE\n\n')

B= stereo_triangulation(xL2,xR2,om,T,fc_left,cc_left,kc_left,alpha_c_left,fc_right,cc_right,kc_right,alpha_c_right)

fprintf('3D co-ords of BLUE RED\n\n')

C= stereo_triangulation(xL3,xR3,om,T,fc_left,cc_left,kc_left,alpha_c_left,fc_right,cc_right,kc_right,alpha_c_right)

fprintf('distance of RG and GB')
 m = sqrt(((A(2) - B(2))^2+ (A(1)-B(1))^2))

fprintf(' \ndistance of RG and BR')
n = sqrt(((A(2) - C(2))^2+ (A(1)-C(1))^2))

fprintf('\n distance of GB and BR')
j = sqrt(((C(2) - B(2))^2+ (C(1)-B(1))^2))

[Pham]

Hi, I'm also interested in this piece of code on the topic: robot arm image processing application. I'm having some problems with code errors. I don't know how I can contact you

Looking forward to your feedback and have a good day❤

Answer 1

[Dima Lisin]

I would recommend using the Stereo Camera Calibrator app and the triangulate function in the Computer Vision System Tooblox. That would simplify your code, and may help you find the problem.