function [wp2, P2, rms] = bundle_adjustment(wp, ip, K, P, pnum)

siz1 = size(P);
siz2 = size(wp);
imgnum = length(pnum);

%%
%% Find optimal world points by nonlinear minimization
%%
param1 = reshape(P, siz1(1)*siz1(2)*siz1(3), 1);
param2 = reshape(wp, siz2(1)*siz2(2), 1);
param = [param1; param2];

options = optimset('fminsearch');
options.MaxFunEvals = 100000;
options.MaxIter = 100000;

param = fminsearch(@comp_reproj, param, options);

e = comp_reproj(param);
rms = sqrt(e / sum(pnum));

param1 = param(1:siz1(1)*siz1(2)*siz1(3));
param2 = param(siz1(1)*siz1(2)*siz1(3)+1:...
               siz1(1)*siz1(2)*siz1(3)+siz2(1)*siz2(2));
P2 = reshape(param1, siz1(1), siz1(2), siz1(3));
wp2 = reshape(param2, siz2(1), siz2(2));

function e = comp_reproj(param)

param1 = param(1:siz1(1)*siz1(2)*siz1(3));
param2 = param(siz1(1)*siz1(2)*siz1(3)+1:...
               siz1(1)*siz1(2)*siz1(3)+siz2(1)*siz2(2));
tmpP = reshape(param1, siz1(1), siz1(2), siz1(3));
tmpwp = reshape(param2, siz2(1), siz2(2));

e = 0;
for m = 1:imgnum
  for n = 1:pnum(m)
    pr = K * tmpP(:,:,m) * [tmpwp(:,n); 1];
    tmpip = pr(1:2) / pr(3);
    dv = tmpip - ip(:,n,m);
    d = sum(dv .* dv);
    e = e + d;
  end
end

end

end
