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

bundle adjustment 玩具程序的更多相关文章

  1. Bundle Adjustment光束平差法概述

    http://blog.csdn.net/abcjennifer/article/details/7588865 http://blog.csdn.net/ximenchuixuezijin/arti ...

  2. 机器人学 —— 机器人视觉（Bundle Adjustment）

    今天完成了机器人视觉的所有课程以及作业,确实是受益匪浅啊! 最后一个话题是Bundle Adjustment. 机器人视觉学中,最顶尖的方法. 1.基于非线性优化的相机位姿估计 之前已经在拟合一篇中, ...

  3. bundle adjustment原理(1)

    那些光束平差的工具,比如SBA.SSBA之类的虽然好,然而例子和教程都不够多且不够详细,让初学者难以上手. 要传入的参数虽然有解释,然而却也不是十分清楚其含义,具体要怎么生成,生成为什么形式. 我在折 ...

  4. bundle adjustment原理(1)转载

    转自菠菜僵尸 http://www.cnblogs.com/shepherd2015/p/5848430.html bundle adjustment原理(1) 那些光束平差的工具,比如SBA.SSB ...

  5. 一个玩具程序——测试密码强度(pure C)

    替人写的C语言作业… 介绍: 程序名称:密码强度检测程序 注释风格:doxygen 测试环境:linux3.6, gcc4.7window7, vs2012 已知问题:1. 算法与参考链接不一致,结果 ...

  6. VIO的Bundle Adjustment推导

    IMU模型和运动积分 $R_{\tiny{WB}} \left( t +\Delta{t} \right) = R_{\tiny{WB}} \left( t \right) Exp\left( \in ...

  7. Linux下安装.bundle后缀的程序

    按照如下操作: #赋予可执行权限 sudo chmod +x XXXXX.bundle #使用root权限启动 sudo ./XXXXX.bundle

  8. ios 中获得应用程序名称和版本号

    IOS程序中的应用名称和版本号在 info.plist 文件中存储着,要想在程序中获得需要使用 NSBundle 对象 下面是示例代码: NSBundle *bundle = [NSBundle ma ...

  9. flask实战-留言板-Web程序开发流程

    Web程序开发流程 在实际的开发中,一个Web程序的开发过程要设计多个角色,比如客户(提出需求).项目经理(决定需求的实现方式).开发者(实现需求)等,在这里我们假设自己是一个人全职开发.一般来说一个 ...

随机推荐

  1. php-isset和empty

    <?php $a = null; if(isset($a)){ echo "is set"."\n"; } else{ echo "not se ...

  2. mvc core2.1 Identity.EntityFramework Core 配置 (一）

    https://docs.microsoft.com/zh-cn/aspnet/core/security/authentication/customize_identity_model?view=a ...

  3. javascript的一些常用知识点

    1. 查看你的html文档中一共有多少个节点 :   document.getElementsByTagName(" * ").length; 2.  document.getEl ...

  4. LeetCode - Merge Two Binary Trees

    Given two binary trees and imagine that when you put one of them to cover the other, some nodes of t ...

  5. expect脚本实例

    #!/usr/bin/expect //必须写这句,表明使用expect,有些系统可能没有自带,使用yum install expect -y 安装 spawn ssh 192.168.2.128 / ...

  6. sqler sql 转rest api 的docker image

    最新sqler 又发布了一个版本,同时官方文档也更新,对于数据库的连接有了详细的说明 Dockerfile 为了方便以及减少大小,使用多阶段构建,同时都通过环境变量运行 FROM alpine:lat ...

  7. 解决GitHub下载速度比较慢

    第一步,打开本机上的Hosts文件 首先,什么是Hosts文件? 在互联网协议中,host表示能够同其他机器互相访问的本地计算机.一台本地机有唯一标志代码,同网络掩码一起组成IP地址,如果通过点到点协 ...

  8. day 33 什么是线程? 两种创建方式. 守护线程. 锁. 死锁现象. 递归锁. GIL锁

    一.线程     1.进程:资源的分配单位    线程:cpu执行单位(实体) 2.线程的创建和销毁开销特别小 3.线程之间资源共享,共享的是同一个进程中的资源 4.线程之间不是隔离的 5.线程可不需 ...

  9. ES查询之刨根问底

    昨天有一个需求,就是想要根据某个网关url做过滤,获取其下面所有的上下文nginx日志:如果直接"query":"https://XXX/YYY/ZZZ"发现有 ...

  10. httpService 和 WebService接口协议

    http协议: 1.调用接口 例1:此例子传输参数为XML文本格式字符串: 将数据缓冲区上载到具有指定 URI 的资源.  var url = "http://localhost:23265 ...