结合 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. 2017-2018-2 20165313实验二《Java面向对象程序设计》

    实验报告封面 实验内容及步骤 实验一 1.试验要求: 参考 (http://www.cnblogs.com/rocedu/p/6371315.html#SECUNITTEST) 完成单元测试的学习. ...

  2. 【mybatis源码学习】mybtias基础组件-占位符解析器

    一.占位符解析器源码 1.占位符解析器实现的目标 通过解析字符串中指定前后缀中的字符,并完成相应的功能. 在mybtias中的应用,主要是为了解析Mapper的xml中的sql语句#{}中的内容,识别 ...

  3. Android USB Host框架

    Android 下的usb框架及功能点:https://blog.csdn.net/tianruxishui/article/details/379029591.Android framework中* ...

  4. day43 数据库学习 转自egon 老师博客 单表查询和多表查询

    一 单表查询的语法 SELECT 字段1,字段2... FROM 表名 WHERE 条件 GROUP BY field HAVING 筛选 ORDER BY field LIMIT 限制条数 二 关键 ...

  5. DOS命令之at命令详解

    AT命令是Windows XP中内置的命令,它也可以媲美Windows中的“计划任务”,而且在计划的安排.任务的管理.工作事务的处理方面,AT命令具有更强大更神通的功能.AT命令可在指定时间和日期.在 ...

  6. tomcat源码阅读之BackupManager

    一. 配置: <Cluster className="org.apache.catalina.ha.tcp.SimpleTcpCluster" channelSendOpti ...

  7. [转]linux中vim命令

    在vi中按u可以撤销一次操作 u      撤销上一步的操作 ctrl+r 恢复上一步被撤销的操作 在vi中移动光标至: 行首:^或0 行尾:$ 页首:1G(或gg) 页尾:G(即shift+g) 显 ...

  8. output.filename 与 output.chunkFilename 的区别

    // webpack.config.js module.exports = { entry: './src/index.js', output: { filename: '[name].bundle. ...

  9. webpack 中的 chunk 种类

    webpack 将 chunk 划分为三类: 入口 chunk.入口 chunk 包含 webpack runtime 和将要加载的模块. 普通 chunk.普通 chunk 不包含 webpack ...

  10. 安装mysql时包冲突解决方法

    报错信息如下: 解决办法: 在卸载代码上加上不检查关联信息即可(rpm -ev mysql-libs-5.1.73-7.el6.x86_64 --nodeps) 检查服务器是否还有mysql安装包:r ...