三维地形图重建代码.
三维重建指定的区域 1
imgDisp=imread('imgDisp.jpg');
2
if isrgb(imgDisp)
3
imgDisp=rgb2gray(imgDisp);
4
end
5
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/2
15
v0=H/2
16
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 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;
29
end
30
X=zeros(rectH,rectW);
31
Y=zeros(rectH,rectW);
32
for 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
42
end
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
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 equal
59
figure
60
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 equal
67
%XYZ_C=[X(:) Y(:) Z(:)];

2

3

4

5

6

7

8

9

10

11

12

13

14

15

16

17

18

19

20

21

22

23

24

25

26

27

28

29

30

31

32

33

34

35

36

37

38

39

40

41

42

43

44

45

46

47

48

49

50

51

52

53

54

55

56

57

58

59

60

61

62

63

64

65

66

67

三维重建全图
1
clc
2
clear
3
close all
4
%读入三位数据,保存到X,Y,Z
5
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 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);
22
end
23
24
%设置栅格大小GridSize,栅格数目NX,NY
25
N=size(X,1);
26
%GridSize=.1;
27
GridSize=(XMAX-XMIN)/50
28
NX=floor((XMAX-XMIN)/GridSize)
29
NY=floor((YMAX-YMIN)/GridSize)
30
if NX>1000 | NY>1000
31
error
32
end
33
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: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
50
end
51
GRID=GRID./CNT;
52
GRID=medfilt2(GRID);
53
%GRID=smooth(GRID);GRID=reshape(GRID,NY,NX);
54
surf(GRIDX,GRIDY,GRID); colorbar
55
%figure
56
%[c,h] = contour(GRIDX,GRIDY,GRID); clabel(c,h), colorbar
57
fprintf('\n%d / %f valid pts\n',NUMOK,N)

2

3

4

5

6

7

8

9

10

11

12

13

14

15

16

17

18

19

20

21

22

23

24

25

26

27

28

29

30

31

32

33

34

35

36

37

38

39

40

41

42

43

44

45

46

47

48

49

50

51

52

53

54

55

56

57

- 上一篇 Mil转为opencv图像格式.
- 下一篇 C++调用matlab.