结合 bundle adjustment原理(1) 和 Levenberg-Marquardt 的 MATLAB 代码 两篇博客的成果，调用MATLAB R2016a中
bundleAdjustment函数的测试程序中的数据的一部分，即“ load('sfmGlobe'); ” 里头的数据，其中17,18,19,20号点的在5个相机
中都能看到，使用这4个点以及5个相机中前面3个相机的数据。假设4个点在这3个相机中都能看到。
数据预处理
clear all;clc;close all;
% K = ...
% [1037.57521466470 0 0;
% 0 1043.31575231792 0;
% 642.231583031218 387.835775096238 1];
%%
% R1 = [0.999930666970964 -0.00673945294088071 -0.00965613924202793;
% 0.00668517563735327 0.999961735652987 -0.00564230950615111;
% 0.00969379583555976 0.00557736532093060 0.999937459703543];
%
% t1 = [-4.60753903591915 -0.701533199568890 0.119971001005677];
% P1 = [R1'; -t1*R1']*K;
% P1 = P1';
% P1 = P1./P1(3,4);
P1 = [-14620.8683429521, 47.7785183087137 -8855.66144393024 -66270.2808443708;
-150.369995939471 -14644.8224529801 -5350.11740074225 -10324.8058386147;
-0.135793602589110 -0.0781294079978937 -14.0074241628698 1]
%%
% R2 = [0.999384955754123,-0.0220100395537247,-0.0272996038647805;
% 0.0221014007388922,0.999751083744459,0.00304936668163826;
% 0.0272256918683320,-0.00365085067123564,0.999622645297548];
%
% t2 = [-5.40461035623086,-0.607149378177483,0.0285909102976046];
% P2 = [R2'; -t2*R2']*K;
% P2 = P2';
% P2 = P2./P2(3,4);
P2 = [9062.69622961192 -216.435747349655 5274.40400172506 48698.1330568296;
288.943296284479 8952.83453874359, 3359.51179040131 6901.28235434353;
0.234003192643106 -0.0313788430818932 8.59169682699799 1]
%%
% R3 = [0.999208035227214,-0.0175748730472845,-0.0356991060776466;
% 0.0183984784259057,0.999569027655827,0.0228747665080092;
% 0.0352816996328509,-0.0235134597317429,0.999100755120554];
%
% t3 = [-5.86393384504139,-0.464881933101877,-0.101272085288487];
%
% P3 = [R3'; -t3*R3']*K;
% P3 = P3';
% P3 = P3./P3(3,4);
P3 = [3565.36981374697 -112.190837623829 2034.77954018230 21061.0035945855;
110.651455202969 3478.99370169127 1384.37501491977 2406.37267504147;
0.118737795949451 -0.0791327065517483 3.36239531623893 1]
clear R1 R2 R3 t1 t2 t3;
P_init = zeros(3,4,3);
P_init(:,:,1) = P1;
P_init(:,:,2) = P2;
P_init(:,:,3) = P3;
%%
X = ...
[-5.02625233070576,2.60296998435244,13.5169230688245;
-6.50729565702308,0.628435271318904,11.8262215983732;
-5.90742477200603,1.96886850306759,13.7807985561123;
-4.56938330250176,2.35488165029457,13.6960613950965];
%%
% 17 在相机1,2,3的坐标
pt1 = ...
[596.60461,635.51575;
639.97058,639.78851;
663.65210,649.69757];
pt2 = ...
[462.77322,497.50113;
514.51044,498.60583;
546.38135,507.36337];
pt3 = ...
[531.15265,585.02240;
573.08856,586.39777;
597.81476,596.26666];
pt4 = ...
[630.10583,613.12250;
673.04602,618.44708;
704.32239,622.03772];
%%
pt1 = pt1';
pt1 = pt1(:);
pt2 = pt2';
pt2 = pt2(:);
pt3 = pt3';
pt3 = pt3(:);
pt4 = pt4';
pt4 = pt4(:);
uvw = ...
[pt1;
pt2;
pt3;
pt4]
clear pt1 pt2 pt3 pt4;
P_init 是 3 x 4 x m 的投影矩阵的数据(m=3)，齐次的，P(3，4) = 1
X 是 4 个点的三维坐标， n x 3 大小 n = 4
uvw 是 所有的二维特征点的坐标，按照[P1X1;P2X1;P3X1; P1X2; P2X2; ... P1X4;P2X4;P3X4]排列，
大小为 24 x 1 ，即 (m x n x 2)
下面是主程序：
clear all;clc;close all;
load('uvw.mat');
load('P_init.mat');
load('points3D.mat');
[p m] = P_to_col(P_init);
clear P_init
[x n] = X_to_col(X);
px = [p; x];
clear p x
%%
syms p x;
p_var = sym_mat(p,11*m);
x_var = sym_mat(x,3*n);
px_var = [p_var;
x_var];
fx = px_var_to_fx(px_var,m,n) - uvw;
% sym_in_fx = symvar(fx) % 变量是全的，排列顺序不理想而已
S = transpose(fx)*fx;
%%
t_init = px;
u = 2;
v = 1.5;
beta = 0.4;
eps = 1.0e-6;
tol = 1;
%%
x = t_init;
jacobian_f = jacobian(fx,px_var);
%%
info_table = [];
k = 0;
while tol>eps
fxk = double(subs(fx,px_var,x));
Sxk = double(subs(S,px_var,x)); % step2: 计算 fxk Sxk
delta_fxk = double(subs(jacobian_f,px_var,x)); % step3: 计算 delta_fxk(J)
delta_Sxk = transpose(delta_fxk)*fxk; % step4: 计算 delta_Sxk
while 1
k = k+1; % 解了一次方程就增加一次计数
% step5: 计算Q，并解方程(Q+uI)delta_x = -delta_Sxk
Q = transpose(delta_fxk)*delta_fxk;
dx = -(Q+u*eye(size(Q)))\delta_Sxk;
x1 = x + dx; % 注意转置
fxk = double(subs(fx,px_var,x1));
Sxk_new = double(subs(S,px_var,x1));
tol = norm(dx); % step6: 计算中止条件 norm(dx)<eps 是否满足，不满足转step 7
tmp = [k tol u Sxk_new] % 记录迭代过程中的信息
info_table = [info_table;tmp];
if tol<=eps
break;
end
% step7:
if Sxk_new < Sxk+beta*transpose(delta_Sxk)*dx
u = u/v;
% disp(['误差减小 成功，u减小为',num2str(u)]);
break;
else
u = u*v;
% disp(['误差减小 失败，u增大为',num2str(u)]);
continue;
end
end
x = x1;
if k>500
break
end
end
调用了
P_to_col，X_to_col，sym_mat，px_var_to_fx几个函数
function [p m] = P_to_col(P_init)
% 把 3 x 4 x m 的 多个 投影矩阵 转换成单独一列的形式
m = size(P_init,3);
p = [];
for k = 1:m
tmp = P_init(:,:,k);
tmp = tmp';
tmp = tmp(:);
tmp(12) = [];
p = [p;tmp];
end
function [x n] = X_to_col(X) % X是三维的点，必须是 n x 3 的形式 n = size(X,1); X = X'; x = X(:);
function rtn = sym_mat(x,m,n)
% 生成符号矩阵，第一个参数是一个符号，后面两个参数是符号矩阵的尺寸
% 如果你想生成符号矩阵[x11 x12; x21 x22]只需输入sym_mat(x,2,2)
% 但事先要先声明符号x,用syms x
% 如果你只需要生成一维矩阵，scjsm会生成一个列向量,如sym_mat(x,2);
% 例子：
% syms x;
% A = sym_mat(x,3,4) 返回一个3 x 4的符号矩阵
if nargin == 2
for i=1:m
rtn(i)=sym([inputname(1),num2str(i)]);
end
rtn = rtn.';
elseif nargin == 3
for i = 1:m
for j = 1:n
rtn(i,j) = sym([inputname(1),num2str(i),num2str(j)]);
end
end
end
function fx = px_var_to_fx(px_var,m,n)
p = px_var(1:11*m);
x = px_var(11*m+1:end);
p = reshape(p,11,m);
p = [p; ones(1,m)];
p_table = [];
for k = 1:m
tmp = p(:,k);
tmp = reshape(tmp,4,3);
tmp = tmp.'; % 3 x 4
p_table = [p_table;
tmp];
end
x = reshape(x,3,n);
x = [x; ones(1,n)];
PX = p_table*x;
idx = (1:m)*3;
w = PX(idx,:);
PX(idx,:) = []; % 现在 PX 的行数刚好是 w 的两倍
w2 = [];
for k = 1:m
ww = repmat(w(k,:),2,1);
w2 = [w2;
ww];
end
fx = PX./w2;
fx = fx(:);
算法迭代500步后投影误差从 110 多降到了 3.83。
程序是有效的，后续还要添加
1，LDL和amd进去
2，某些点在某些相机看不到的情况
3，对稀疏矩阵分块，然后求解的步骤
4，不使用jacobian函数，J矩阵根据另一篇博客推导出的公式计算
1705
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
