num = 12;
tnum = 13;
filenames = ['image_points_01.txt';
             'image_points_02.txt';
             'image_points_03.txt'];
siz = size(filenames);
imgnum = siz(1);
clear ip;
for m = 1:imgnum
  A = load(filenames(m,:));
  ip(:,:,m) = A(1:num,:)';
  ipt(:,:,m) = A(tnum,:)';
end

B = load('world_points.txt');
wp = B(1:num,:)';
wpt = B(tnum,:)';
siz2 = size(wp);

filenames = {'intrinsic_parallel.txt',
             'intrinsic_planes.txt', %principal point is modified
                                     %for zero-origin coordinate
             'intrinsic_indoor.txt',
             'intrinsic_outdoor.txt'};
siz = size(filenames);
knum = siz(1);

for k = 1:knum
  fprintf(1, 'K: %s\n', filenames{k});
  K = load(filenames{k});
  
  for m = 1:imgnum
    x_ext = ip(:,:,m);
    X_ext = wp;
    P(:,:,m) = estimate_extparam2(x_ext, X_ext, K);
    pnum(m) = num;
  end

  [wp2, P2, rms] = bundle_adjustment(wp, ip, K, P, pnum);
  fprintf(1, 'Fitting Error: RMS = %3.8f pixels\n', rms);

  figure
  hold on
  plot3(wp2(1,:), wp2(2,:), wp2(3,:), 'g+');

  clear vipt t p
  for m = 1:imgnum
    vipt(:,m) = inv(P2(:,1:3,m)) * inv(K) * [ipt(:,:,m); 1];
    t(:,m) = - inv(P2(:,1:3,m)) * P2(:,4,m);
    p(:,2*m-1) = vipt(:,m) * 0 + t(:,m);
    p(:,2*m)   = vipt(:,m) * 5 + t(:,m);
  end
  mwp = midpoint(vipt, t);

  for m = 1:imgnum
    plot3(p(1,2*m-1), p(2,2*m-1), p(3,2*m-1), 'r*');
    plot3(p(1,2*m-1:2*m), p(2,2*m-1:2*m), p(3,2*m-1:2*m), 'r-');
  end
  plot3(mwp(1), mwp(2), mwp(3), 'bo');
  xlabel('X (m)', 'FontName', 'Helvetica', ...
         'FontSize', 12);
  ylabel('Y (m)', 'FontName', 'Helvetica', ...
         'FontSize', 12);
  zlabel('Z (m)', 'FontName', 'Helvetica', ...
         'FontSize', 12);
  set(gca,'FontName', 'Helvetica')

  axis equal

  rms = 0;
  clear mipt;
  for m = 1:imgnum
    tmp = K * P2(:,:,m) * [mwp; 1];
    mipt(:,m) = tmp(1:2) / tmp(3);
    dip = mipt(:,m) - ipt(:,:,m);
    rms = rms + sum(dip .* dip);
  end
  rms = sqrt(rms / imgnum);
  fprintf(1, 'Reprojection Error: RMS = %3.8f pixels\n', rms);
end
