Matlab: триангуляционная функция Matlab дает неправильные трехмерные координаты

Question

Я сталкиваюсь со странной ошибкой при использовании триангуляционной функции Matlab. Несколько месяцев назад все работало нормально. Ошибка, скажем, у меня есть два белых объекта ( W1 и W2) на левом и правом изображениях, снятых с помощью стереокамер (уже откалиброван и ссылка на файл StereoParams дана в конце), как показано ниже:

правое изображение:

левое изображение:

Функциятриангуляции дает только правильное положение самого левого белого объекта, W1. Но если я накрыл W1, а затем выяснил трехмерное положение W2, ошибки не было, и положение W2 вышло правильно.

Когда я использовал выше изображения, результат выглядит так:

worldPoints
   1.0e+03 *

   -0.0605    0.0322    0.6477
    0.1109   -0.0603   -1.1875

В этом 1.0e+03 * (-0.0605 0.0322 0.6477)расстояние корректно в пределах 6-7 мм, но другое расстояние совершенно неверно. Поскольку оба белых объекта находятся в одной плоскости, расстояние Z должно быть 647,7 мм для обоих объектов. Но это другое.

Теперь, если покрытый объект W1 и попытаться выяснить трехмерное положение объекта W2, он выглядит как

-249.3425   30.3535  649.0859

что правильно.

Дело в том, что если в стереоизображениях присутствует один объект, triangulate function Точный расчет 3D-точек, но для нескольких объектов правильными являются только левые большинство объектов.

Мой код ниже:

right_img1 = imread('right150.png');
left_img1 = imread('left150.png');
%function [Pick_points] = triangulationfun(left_img,right_img)
load('stereoParams.mat');
right_img = undistortImage(right_img1, stereoParams.CameraParameters2);
left_img = undistortImage(left_img1, stereoParams.CameraParameters1);

I2 = right_img;
thresh = 0.6;
bwredFrame2 = im2bw(I2(:,:,1), thresh); % obtain the white component from red layer
bwgreenFrame2 = im2bw(I2(:,:,2), thresh); % obtain the white component from green layer
bwblueFrame2 = im2bw(I2(:,:,3), thresh); % obtain the white component from blue layer
bw2 = bwredFrame2 & bwgreenFrame2 & bwblueFrame2; % get the common region

boll2 = bwareaopen(bw2,1125);
boll2 = imfill(boll2,'holes');

% to subtract the area having pixels less than thresholding
% for first image
bollsMeasurements2 = regionprops(boll2, 'all');
%numberOfBolls1 = size(bollsMeasurements1, 1);

if isempty(bollsMeasurements2)
    %error('No ball found!');
    centroids_cotton_right = [0,0];
else
    centroids_cotton_right = cat(1, bollsMeasurements2.Centroid);
end

I1 = left_img;
thresh = 0.6;
bwredFrame1 = im2bw(I1(:,:,1), thresh); % obtain the white component from red layer
bwgreenFrame1 = im2bw(I1(:,:,2), thresh); % obtain the white component from green layer
bwblueFrame1 = im2bw(I1(:,:,3), thresh); % obtain the white component from blue layer
bw1 = bwredFrame1 & bwgreenFrame1 & bwblueFrame1; % get the common region

boll1 = bwareaopen(bw1,1125);
boll1 = imfill(boll1,'holes');

bollsMeasurements1 = regionprops(boll1, 'all');

if isempty(bollsMeasurements1)
    %error('No ball found!');
    centroids_cotton_left = [0,0];
else
    centroids_cotton_left = cat(1, bollsMeasurements1.Centroid);
end

B = centroids_cotton_left; % centroids for left image
A = centroids_cotton_right; % centroids for right image

disp('centroids of left img')
disp(B);
disp('centroids of right img')
disp(A);

%stereoParams = load('D:\phd work\testing of robotic arm\final ik using peter coke\stereoParams.mat');

dists = pdist2( A(:,2), B(:,2) );
%dists ( bsxfun(@gt, dists, min(dists,[],1)) | bsxfun(@gt, dists, min(dists,[],2)) ) = Inf;

[iA, iB] = find( dists < 5 );
right_cor = A(iA, :);  % right coordinates
left_cor = B(iB, :);   % left coordinates

[worldPoints,reprojectionErrors] = triangulate(left_cor,right_cor,stereoParams);

validIdx = reprojectionErrors < 1;
worldPoints = worldPoints(validIdx, :);

disp('worldPoints')
disp(worldPoints);

Файл StereoParams и изображения приведены в этой ссылке.

Почему эта проблема? Как это решить? Я много раз калибровал стереокамеры, но результат тот же.

Благодарю.