close all; clear all; close all clc; im = imread('ROIL.png'); im=rgb2gray(im); disp=imread('dispL.png'); [r,c]=size(disp); disp=double(disp); q = [1. 0. 0. -1.5690376663208008e+002;... 0. 1. 0. -1.4282237243652344e+002;... 0. 0. 0. 5.2004731331639300e+002;... 0. 0. 1.0945105843175637e-002 0.]; % q(4,4) 原为负值,现修正为正值 big_z = 1e5; tmp=zeros(4,1); for i = 1:r for j = 1:c if disp(i,j)>0 % Textbook method tmp = q*[j,i,disp(i,j),1]'; % j 是列数,i 是行数,分别对应公式中的 x 和 y pos1(i,j,:) = tmp(1:3)/tmp(4); else pos1(i,j,3) = big_z; end end end subplot(121); imshow(im); title('Left Frame'); subplot(122); imshow(uint8(disp)); title('Disparity map'); % Matlab 按公式直接计算得到的三维坐标 x = pos1(:,:,1); y = -pos1(:,:,2); z = pos1(:,:,3); ind = find(z>10000); % 以毫米为量纲 x(ind)=NaN; y(ind)=NaN; z(ind)=NaN; figure, %mesh(x,z,y,double(im),'FaceColor','texturemap'); %colormap(gray); x=x(:)'; y=y(:)'; z=z(:)'; [a1,b1]=max(x); [a2,b2]=min(x); k1max=round(a1); k1min=round(a2); [a3,b3]=max(y); [a4,b4]=min(y); k2max=round(a3); k2min=round(a4); [a5,b5]=max(z); [a6,b6]=min(z); k3max=round(a5); k3min=round(a6); scatter3(x,z,y,'.'); axis equal; %axis([-100 1000 0 1000 -1000 100]); axis([k1min k1max k3min k3max k2min k2max]); xlabel('Horizonal');ylabel('Depth');zlabel('Vertical'); title('Equation method'); %view([0 0]); % 正视图 % view([0 90]); % 俯视图 % view([90 0]); % 侧视图