这里采用的是Yi Ma , Stefano Soatto. An Invitation to 3-D Vision , From Images to Geometric Models 的算法
![](https://i-blog.csdnimg.cn/blog_migrate/0479dfee29266cbf8b03e467a67aa880.jpeg)
![](https://i-blog.csdnimg.cn/blog_migrate/b8e259d643ecdb316593157bd847cb9a.jpeg)
%
//
Algorithm 8.1. also 11.7
%
//
Rank based factorization algorithm for multiview reconstruction
%
//
using point features
%
//
as described in Chapter 8, "An introduction to 3-D Vision"
%
//
by Y. Ma, S. Soatto, J. Kosecka, S. Sastry (MASKS)
%
//
Code distributed free for non-commercial use
%
//
Copyright (c) MASKS, 2003
%
//
Generates multiple synthetic views of a house and computes the
%
//
motion and structure, calibrated case, point features only
%
//
Jana Kosecka, George Mason University, 2002
%
//
======================================================================
close all; clear;
FRAMES
=
3
;
PLOTS
=
3
;
%
//
transformation is expressed wrt to the camera frame
Zinit
=
5
;
![None.gif](https://www.cnblogs.com/Images/OutliningIndicators/None.gif)
%
//
cube in the object frame
XW
=
[
0
1
1
0
0
1
1
0
0.2
0.8
0.2
0.8
;
0
0
1
1
0
0
1
1
1.5
1.5
1.5
1.5
;
1
1
1
1
0
0
0
0
0.8
0.8
0.2
0.2
;
1
1
1
1
1
1
1
1
1
1
1
1
];
![None.gif](https://www.cnblogs.com/Images/OutliningIndicators/None.gif)
NPOINTS
=
12
;
![None.gif](https://www.cnblogs.com/Images/OutliningIndicators/None.gif)
XC
=
zeros(
4
,NPOINTS,FRAMES);
![None.gif](https://www.cnblogs.com/Images/OutliningIndicators/None.gif)
%
//
initial displacement摄像机的初始位置
Rinit
=
rot_matrix([
1
1
1
],
0
);
![None.gif](https://www.cnblogs.com/Images/OutliningIndicators/None.gif)
Tinit
=
[ Rinit(
1
,:)
-
0.5
;
Rinit(
2
,:)
-
0.5
;
Rinit(
3
,:) Zinit;
0
0
0
1
];
%
//
first camera coodinates
XC(:,:,
1
)
=
Tinit
*
XW;
![None.gif](https://www.cnblogs.com/Images/OutliningIndicators/None.gif)
%
//
画出三维的结构 original motion and 3D structure
figure; hold on;
plot3_struct(XC(
1
,:,
1
),XC(
2
,:,
1
),XC(
3
,:,
1
));
plot3(XC(
1
,:,
1
),XC(
2
,:,
1
),XC(
3
,:,
1
),
'
*
'
);
draw_frame_scaled([diag([
1
,
1
,
1
]), zeros(
3
,
1
)],
0.5
);
title(
'
original motion and 3D structure
'
);
view(
220
,
20
);
grid on; axis equal;
%
//
axis off;
pause;
![None.gif](https://www.cnblogs.com/Images/OutliningIndicators/None.gif)
![None.gif](https://www.cnblogs.com/Images/OutliningIndicators/None.gif)
%
//
image coordinates 计算第一帧时的图像坐标
xim(:,:,
1
)
=
project(XC(:,:,
1
));
![None.gif](https://www.cnblogs.com/Images/OutliningIndicators/None.gif)
Zmax
=
max(XC(
3
,:,
1
));
Zmin
=
min(XC(
3
,:,
1
));
rinc
=
pi
/
30
;
rot_axis
=
[
1
0
0
;
0
-
1
0
]
'
;
trans_axis
=
[
1
0
0
;
0
1
0
]
'
;
ratio
=
1
;
rinc
=
10
;
%
//
rotation increment 20 degrees
Zmid
=
(Zmax
+
Zmin)
/
2
;
tinc
=
0.5
*
ratio
*
Zmid
*
rinc
*
pi
/
180
;
![None.gif](https://www.cnblogs.com/Images/OutliningIndicators/None.gif)
ploting
=
1
;
![None.gif](https://www.cnblogs.com/Images/OutliningIndicators/None.gif)
for
i
=
2
:FRAMES
%
//
计算第i帧的图像坐标xim
theta
=
(i
-
1
)
*
rinc
*
pi
/
180
;
r_axis
=
rot_axis(:,i
-
1
)
/
norm(rot_axis(:,i
-
1
));
t_axis
=
trans_axis(:,i
-
1
)
/
norm(trans_axis(:,i
-
1
));
trans
=
(i
-
1
)
*
tinc
*
t_axis;
R
=
rot_matrix(r_axis,theta);
%
//
translation represents origin of the camera frame
%
//
in the world frame
T(:,:,i)
=
([ R trans;
0
0
0
1
]);
%
//
all transformation with respect to the object frame
XC(:,:,i)
=
T(:,:,i)
*
XC(:,:,
1
);
%
//
XW;
draw_frame_scaled(T(
1
:
3
,:,i),
0.5
);
xim(:,:,i)
=
[XC(
1
,:,i).
/
XC(
3
,:,i); XC(
2
,:,i).
/
XC(
3
,:,i); ![dot.gif](https://www.cnblogs.com/Images/dot.gif)
ones(
1
,NPOINTS)];
end;
![None.gif](https://www.cnblogs.com/Images/OutliningIndicators/None.gif)
for
j
=
2
:FRAMES
T_ini(:,j)
=
T(
1
:
3
,
4
,j);
end;
![None.gif](https://www.cnblogs.com/Images/OutliningIndicators/None.gif)
%
//
noise can be added here
for
i
=
1
:FRAMES
xim_noisy(:,:,i)
=
xim(:,:,i);
end
![None.gif](https://www.cnblogs.com/Images/OutliningIndicators/None.gif)
%
//
pause 以下为SFM算法
%
//
---------------------------------------------------------------------
%
//
compute initial \alpha's for each point using first two frames only 1)首先用八点算法计算初始的R0,T0(我感觉T0~即1,0帧之间的相对移动~和实际的应该相差常数倍,因此会导致恢复的结构和实际相差常数倍),然后估计lambda。。。
[T0, R0]
=
essentialDiscrete(xim_noisy(:,:,
1
),xim_noisy(:,:,
2
));
for
i
=
1
:NPOINTS
alpha(:,i)
=
-
(skew(xim_noisy(:,i,
2
))
*
T0)
'
*
(skew(xim_noisy(:,i,
2
))
*
R0
*
xim_noisy(:,i,
1
))![dot.gif](https://www.cnblogs.com/Images/dot.gif)
/
(norm(skew(xim_noisy(:,i,
2
))
*
T0))
^
2
;
lambda(:,i)
=
1
/
alpha(:,i);
end
![None.gif](https://www.cnblogs.com/Images/OutliningIndicators/None.gif)
scale
=
norm(alpha(:,
1
));
%
//
set the global scale
alpha
=
alpha
/
scale;
%
//
normalize everything
scale
=
norm(lambda(:,
1
));
%
//
set the global scale
lambda
=
lambda
/
scale;
%
//
normalize everything
%
//
---------------------------------------------------------------------
%
//
Compute initial motion estimates for all frames
%
//
Here do 3 iterations - in real setting look at the change of scales
iter
=
1
;
while
(iter
<
5
);
for
j
=
2
:FRAMES
P
=
[];
%
//
setup matrix P
for
i
=
1
:NPOINTS
a
=
[kron(skew(xim_noisy(:,i,j)),xim(:,i,
1
)
'
)
alpha(:,i)
*
skew(xim_noisy(:,i,j))];
P
=
[P; a];
end;
%
//
pause
[um, sm, vm]
=
svd(P);
Ti
=
vm(
10
:
12
,
12
);
Ri
=
transpose(reshape(vm(
1
:
9
,
12
)
'
,3,3));
[uu,ss,vv]
=
svd(Ri);
Rhat(:,:,j)
=
sign(det(uu
*
vv
'
))*uu*vv
'
;
Ti
=
sign(det(uu
*
vv
'
))*Ti/((det(ss))^(1/3));
That(:,j)
=
Ti;
True
=
T(
1
:
3
,
4
,j);
end
![None.gif](https://www.cnblogs.com/Images/OutliningIndicators/None.gif)
%
//
recompute alpha's based on all views
lambda_prev
=
lambda;
for
i
=
1
:NPOINTS
M
=
[];
%
//
setup matrix M
for
j
=
2
:FRAMES
%
//
set up Hl matrix for all m views
a
=
[ skew(xim(:,i,j))
*
That(:,j) ![dot.gif](https://www.cnblogs.com/Images/dot.gif)
skew(xim(:,i,j))
*
Rhat(:,:,j)
*
xim(:,i,
1
)];
M
=
[M; a];
end;
a1
=
-
M(:,
1
)
'
*M(:,2)/norm(M(:,1))^2;
lambda(:,i)
=
1
/
a1;
end;
scale
=
norm(lambda(:,
1
));
%
//
set the global scale
lambda
=
lambda
/
scale;
%
//
normalize everything
iter
=
iter
+
1
end
%
//
end while iter
%
//
final structure with respect to the first frame
XF
=
[lambda.
*
xim(
1
,:,
1
);
lambda.
*
xim(
2
,:,
1
);
lambda.
*
xim(
3
,:,
1
)];
![None.gif](https://www.cnblogs.com/Images/OutliningIndicators/None.gif)
figure; hold on;
plot3(XF(
1
,:,
1
),XF(
2
,:,
1
),XF(
3
,:,
1
),
'
r*
'
);
plot3_struct(XF(
1
,:,
1
), XF(
2
,:,
1
), XF(
3
,:,
1
));
title(
'
recovered structure
'
);
view(
220
,
20
);
grid on; axis equal;
%
//
axis off;
pause;