三维地形图重建代码.
三维重建指定的区域 1
imgDisp=imread('imgDisp.jpg');
2if isrgb(imgDisp)
3 imgDisp=rgb2gray(imgDisp);
4end
5
6imshow(uint8(4*double(imgDisp)))
7disp('选择一个需要三维重建的区域，先左上角，再右下角')
8rect=ginput(2)
9
10%3d重建
11base=0.27;
12f=740;
13[H,W]=size(imgDisp);
14u0=W/2
15v0=H/2
16rectDisp=imgDisp(rect(1,2):rect(2,2),rect(1,1):rect(2,1));
17[rectH,rectW]=size(rectDisp);
18imshow(rectDisp)
19rectDisp=double(rectDisp);
20Z=zeros(rectH,rectW);
21if 0
22 [row,col]=find(rectDisp==0);
23 Z=f*base./rectDisp;
24 Z(row,col)=0;
25 Z2=Z;
26 [u,v]=meshgrid(rect(1,1):rect(2,1),rect(1,2):rect(2,2));
27 X2=Z.*(u-u0)/f;
28 Y2=Z.*(v-v0)/f;
29end
30X=zeros(rectH,rectW);
31Y=zeros(rectH,rectW);
32for x=1:rectW
33 for y=1:rectH
34 if rectDisp(y,x)==0
35 Z(y,x)=0;
36 else
37 Z(y,x)=f*base/rectDisp(y,x);
38 X(y,x)=Z(y,x)*(x+rect(1,1)-1-u0)/f;
39 Y(y,x)=Z(y,x)*(y+rect(1,2)-1-v0)/f;
40 end
41 end
42end
43% Z_Z2=norm(Z-Z2)
44% X_X2=norm(X-X2)
45% Y_Y2=norm(Y-Y2)
46%for y=2:(H-1)
47
48Y=medfilt2(Y);Y=medfilt2(Y);
49X=medfilt2(X);
50Z=medfilt2(Z);
51Z=Z/2;
52%surf(Y)
53
54plot3(X(:),Z(:),-Y(:),'.')
55xlabel('x/(m)')
56ylabel('y/(m)')
57zlabel('z/(m)')
58axis equal
59figure
60
61surf(X(2:(rectH-1),2:(rectW-1)),Z(2:(rectH-1),2:(rectW-1)),-Y(2:(rectH-1),2:(rectW-1)))
62xlabel('x/(m)')
63ylabel('y/(m)')
64zlabel('z/(m)')
65
66axis equal
67%XYZ_C=[X(:) Y(:) Z(:)];
imgDisp=imread('imgDisp.jpg');2
if isrgb(imgDisp)3
imgDisp=rgb2gray(imgDisp);4
end5
6
imshow(uint8(4*double(imgDisp)))7
disp('选择一个需要三维重建的区域，先左上角，再右下角')8
rect=ginput(2)9
10
%3d重建11
base=0.27;12
f=740;13
[H,W]=size(imgDisp);14
u0=W/215
v0=H/216
rectDisp=imgDisp(rect(1,2):rect(2,2),rect(1,1):rect(2,1));17
[rectH,rectW]=size(rectDisp);18
imshow(rectDisp)19
rectDisp=double(rectDisp);20
Z=zeros(rectH,rectW);21
if 022
[row,col]=find(rectDisp==0);23
Z=f*base./rectDisp;24
Z(row,col)=0;25
Z2=Z;26
[u,v]=meshgrid(rect(1,1):rect(2,1),rect(1,2):rect(2,2));27
X2=Z.*(u-u0)/f;28
Y2=Z.*(v-v0)/f;29
end30
X=zeros(rectH,rectW);31
Y=zeros(rectH,rectW);32
for x=1:rectW33
for y=1:rectH34
if rectDisp(y,x)==035
Z(y,x)=0;36
else37
Z(y,x)=f*base/rectDisp(y,x);38
X(y,x)=Z(y,x)*(x+rect(1,1)-1-u0)/f;39
Y(y,x)=Z(y,x)*(y+rect(1,2)-1-v0)/f; 40
end41
end42
end43
% Z_Z2=norm(Z-Z2)44
% X_X2=norm(X-X2)45
% Y_Y2=norm(Y-Y2)46
%for y=2:(H-1)47
48
Y=medfilt2(Y);Y=medfilt2(Y);49
X=medfilt2(X);50
Z=medfilt2(Z);51
Z=Z/2;52
%surf(Y)53
54
plot3(X(:),Z(:),-Y(:),'.')55
xlabel('x/(m)')56
ylabel('y/(m)')57
zlabel('z/(m)')58
axis equal59
figure60
61
surf(X(2:(rectH-1),2:(rectW-1)),Z(2:(rectH-1),2:(rectW-1)),-Y(2:(rectH-1),2:(rectW-1)))62
xlabel('x/(m)')63
ylabel('y/(m)')64
zlabel('z/(m)')65
66
axis equal67
%XYZ_C=[X(:) Y(:) Z(:)];三维重建全图
1clc
2clear
3close all
4%读入三位数据，保存到X,Y,Z
5XYZ=load('3dpts.txt');
6X=XYZ(:,1+3);
7Y=XYZ(:,3+3);
8Z=-XYZ(:,2+3);
9fprintf('X %d %d\n',max(X),min(X))
10fprintf('Y %d %d\n',max(Y),min(Y))
11fprintf('Z %d %d\n',max(Z),min(Z))
12
13%设置显示的范围
14M=.5;
15XMIN=max(min(X)*M,-5);XMAX=min(max(X)*M,5);
16YMIN=max(min(Y)/M,1);YMAX=min(max(Y)*M,10);
17ZMIN=max(min(Z)*M,-1);ZMAX=min(max(Z)*M,10);
18if 1
19 XMIN=max(min(X)*M,-5);XMAX=min(max(X)*M,5);
20 YMIN=max(min(Y)*M,1);YMAX=min(max(Y)*M,10);
21 ZMIN=max(min(Z)*M,-10);ZMAX=min(max(Z)*M,100);
22end
23
24%设置栅格大小GridSize，栅格数目NX，NY
25N=size(X,1);
26%GridSize=.1;
27GridSize=(XMAX-XMIN)/50
28NX=floor((XMAX-XMIN)/GridSize)
29NY=floor((YMAX-YMIN)/GridSize)
30if NX>1000 | NY>1000
31 error
32end
33
34%构造栅格
35[GRIDX,GRIDY]=meshgrid((1:NX)*GridSize,(1:NY)*GridSize);
36GRID=zeros(NY,NX);
37CNT=ones(NY,NX);
38XYZ=[floor((X-XMIN)/GridSize+.5) floor((Y-YMIN)/GridSize+.5) (Z+0.5)];
39NUMOK=0;
40for i=1:N
41 if(rem(i,100)==0) fprintf('%d ',i);end
42 ix=XYZ(i,1);
43 iy=XYZ(i,2);
44 iz=XYZ(i,3);
45 if ix>0 & ix<=NX & iy>0 & iy<=NY & iz>ZMIN & iz<ZMAX
46 GRID(iy,ix)=GRID(iy,ix)+iz;
47 CNT(iy,ix)=CNT(iy,ix)+1;
48 NUMOK=NUMOK+1;
49 end
50end
51GRID=GRID./CNT;
52GRID=medfilt2(GRID);
53%GRID=smooth(GRID);GRID=reshape(GRID,NY,NX);
54surf(GRIDX,GRIDY,GRID); colorbar
55%figure
56%[c,h] = contour(GRIDX,GRIDY,GRID); clabel(c,h), colorbar
57fprintf('\n%d / %f valid pts\n',NUMOK,N)
clc2
clear3
close all4
%读入三位数据，保存到X,Y,Z5
XYZ=load('3dpts.txt'); 6
X=XYZ(:,1+3);7
Y=XYZ(:,3+3);8
Z=-XYZ(:,2+3);9
fprintf('X %d %d\n',max(X),min(X))10
fprintf('Y %d %d\n',max(Y),min(Y))11
fprintf('Z %d %d\n',max(Z),min(Z))12
13
%设置显示的范围14
M=.5;15
XMIN=max(min(X)*M,-5);XMAX=min(max(X)*M,5);16
YMIN=max(min(Y)/M,1);YMAX=min(max(Y)*M,10);17
ZMIN=max(min(Z)*M,-1);ZMAX=min(max(Z)*M,10);18
if 119
XMIN=max(min(X)*M,-5);XMAX=min(max(X)*M,5);20
YMIN=max(min(Y)*M,1);YMAX=min(max(Y)*M,10);21
ZMIN=max(min(Z)*M,-10);ZMAX=min(max(Z)*M,100);22
end23
24
%设置栅格大小GridSize，栅格数目NX，NY25
N=size(X,1);26
%GridSize=.1;27
GridSize=(XMAX-XMIN)/5028
NX=floor((XMAX-XMIN)/GridSize)29
NY=floor((YMAX-YMIN)/GridSize)30
if NX>1000 | NY>100031
error32
end33
34
%构造栅格35
[GRIDX,GRIDY]=meshgrid((1:NX)*GridSize,(1:NY)*GridSize);36
GRID=zeros(NY,NX);37
CNT=ones(NY,NX);38
XYZ=[floor((X-XMIN)/GridSize+.5) floor((Y-YMIN)/GridSize+.5) (Z+0.5)];39
NUMOK=0;40
for i=1:N41
if(rem(i,100)==0) fprintf('%d ',i);end42
ix=XYZ(i,1);43
iy=XYZ(i,2);44
iz=XYZ(i,3);45
if ix>0 & ix<=NX & iy>0 & iy<=NY & iz>ZMIN & iz<ZMAX46
GRID(iy,ix)=GRID(iy,ix)+iz;47
CNT(iy,ix)=CNT(iy,ix)+1; 48
NUMOK=NUMOK+1;49
end50
end51
GRID=GRID./CNT;52
GRID=medfilt2(GRID);53
%GRID=smooth(GRID);GRID=reshape(GRID,NY,NX);54
surf(GRIDX,GRIDY,GRID); colorbar55
%figure56
%[c,h] = contour(GRIDX,GRIDY,GRID); clabel(c,h), colorbar57
fprintf('\n%d / %f valid pts\n',NUMOK,N)- 上一篇 Mil转为opencv图像格式.
- 下一篇 C++调用matlab.