结合 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. hdu3613 Best Reward manacher+贪心+前缀和

    After an uphill battle, General Li won a great victory. Now the head of state decide to reward him w ...

  2. CentOS下如何根据Dump文件分析线上问题

    https://blog.csdn.net/lixin03080/article/details/79711296 一.保存现场 1.记录系统整体资源使用情况,进程信息.线程信息 top -b -n ...

  3. mysql export query result

    1 . export by shell a.sql use dbname; SELECT id,iab_num FROM iab_list ; mysql -h host -uusername -P3 ...

  4. linux I/O状态实时监控iostat

    首先查看系统有没有安装sysstat 如果没有,则yum install sysstat -y [root@bogon ~]# iostat -c 2 2 #显示cpu状态信息 Linux 3.10. ...

  5. fcntl获取和修改文件打开状态标志

    [root@bogon code]# cat b.c #include<stdio.h> #include<error.h> #include<unistd.h> ...

  6. gogs wekan 集成试用

    wekan 官方提供了一个集成gogs 的扩展,不是完全的自动化,需要结合cli,但是官方的cli 写的...(不是很全) 备注: 测试环境使用docker-compose 环境准备 docker-c ...

  7. 【网络协议】TCP分段与IP分片

    版权声明:本文为博主原创文章,未经博主同意不得转载. https://blog.csdn.net/mmc_maodun/article/details/30109789     我们在学习TCP/IP ...

  8. 使用dynamic和MEF实现轻量级的AOP组件 ---- 系列文章

      .NET 4 实践 - 使用dynamic 和MEF实现轻量级的AOP组件(1)   .NET 4 实践 - 使用dynamic和MEF实现轻量级的AOP组件 (2) .NET 4 实践 - 使用 ...

  9. oracle-对象表-外部表

    http://www.blogjava.net/decode360/archive/2008/10/16/286802.html create or replace type person as ob ...

  10. [转]一致性hash算法 - consistent hashing

    consistent hashing 算法早在 1997 年就在论文 Consistent hashing and random trees 中被提出,目前在 cache 系统中应用越来越广泛: 1  ...