zoukankan      html  css  js  c++  java
  • 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矩阵根据另一篇博客推导出的公式计算

  • 相关阅读:
    防删没什么意思啊,直接写废你~
    绝大多数情况下,没有解决不了的问题,只有因为平时缺少练习而惧怕问题的复杂度,畏惧的心理让我们选择避让,采取并不那么好的方案去解决问题
    Java 模拟面试题
    Crossthread operation not valid: Control 'progressBar1' accessed from a thread other than the thread it was created on
    一步步从数据库备份恢复SharePoint Portal Server 2003
    【转】理解 JavaScript 闭包
    Just For Fun
    The database schema is too old to perform this operation in this SharePoint cluster. Please upgrade the database and...
    Hello World!
    使用filter筛选刚体碰撞
  • 原文地址:https://www.cnblogs.com/shepherd2015/p/5867362.html
Copyright © 2011-2022 走看看