bundle adjustment 玩具程序

结合 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矩阵根据另一篇博客推导出的公式计算