三维地形图重建代码

三维重建指定的区域

 1imgDisp=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(:)];

三维重建全图

 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)