clear
close all
load Calib_Results

rgb = imread('image2.jpg');
imshow(rgb);

ndx = find(X_2(2,:)==0);
ndy = find(X_2(1,:)==0);
mx = find(and(X_2(2,:)==0,X_2(1,:)==21));
my = find(and(X_2(2,:)==21,X_2(1,:)==0));

hold on
plot(x_2(1,ndx),x_2(2,ndx),'b','LineWidth',2);
plot(x_2(1,ndy),x_2(2,ndy),'y','LineWidth',2);
text(x_2(1,mx),x_2(2,mx),'X','Color','b','FontSize',21);
text(x_2(1,my),x_2(2,my),'Y','Color','y','FontSize',21);
hold off

%-- Image #2:
% omc_2 = [ 8.483120e-01 ; 2.146933e+00 ; -1.427449e+00 ];
% Tc_2  = [ 1.003981e+01 ; 5.265841e+00 ; 1.323915e+02 ];

R = rodrigues(omc_2)

Tw = -R'*Tc_2/12

R =
   -0.7234    0.6902    0.0139
    0.2542    0.2851   -0.9242
   -0.6419   -0.6650   -0.3817
Tw =
    7.5754
    6.6345
    4.6051

Tc = [-5.83074 6.61428 -4.59727] % MATLAB result

[ -Tw(1)+21/12 Tw(2) -Tw(3)]   % convert Bouguet to MATLAB

Tc =
   -5.8307    6.6143   -4.5973
ans =
   -5.8254    6.6345   -4.6051

Published with MATLAB® R2019b