example_res.m

数学优化工具箱

U=[ -244.1987   25.5719 -359.4652
    -379.5291    2.7389 -214.0868
    -388.0328    0.0463 -122.0990
    -229.4412  456.6910  -41.9321
    -254.9794  330.4571 -196.8263
    -466.7806  487.5593  -20.2237
     103.5358   16.2488  477.2610
    -252.8017  168.1946 -353.2684
     -94.8777 -357.9471 -155.7540
      49.9321 -457.5683 -268.0401
    -101.2093 -379.3090  -57.3699
    -448.5477  496.6466  194.3763
     115.0217  117.1546  329.7392
     479.2856  -62.5653  346.2103
    -102.9164  -79.6282  391.9881
    -154.3383  308.4130  494.2763
     281.0638  351.8088  160.6110
    -227.8873 -259.1568  -74.0764
      97.5581  -79.9588  267.5242
    -474.3083  192.7930  253.4382]; %noisefree 3d points


u=[ -157.2652 -283.6496
     -35.7850 -210.4401
      -2.8299 -143.3957
    -318.7760   29.8652
    -296.4039  -95.1415
    -240.7724  -46.9992
      32.6728  530.3988
    -258.2445 -246.0239
     260.2755  -98.4758
     355.8482 -171.9631
     314.4578   -4.4008
    -189.1177   70.0006
     -98.1606  451.8460
     -80.6440  923.0146
     131.9544  335.5002
     -89.6580  342.3272
    -422.4504  475.4933
     195.2204  -75.8743
      49.9415  417.2676
      -8.1229   72.1127]; % noisy image observations

 P0=[   -0.3305   -0.9074    0.3648  -87.1533
         0.5826    0.1277    0.8428  151.9859
        -0.0008    0.0005    0.0005    1.0000]; % noisefree estimate of the camera matrix
    

epsilon = 0.95; % relative termination tolerance.
delta = 15; % upperbound on the error;
maxiter=100;

addpath 'l2l2'
addpath 'l2l1'
addpath 'l1l1'

Pl2l2  = l2l2_resection(u',U',epsilon,delta,maxiter);
Pl2l1  = l2l1_resection(u',U',epsilon,delta,maxiter);
Pl1l1  = l1l1_resection(u',U',epsilon,delta,maxiter);

fprintf('ground truth\n')
P0/P0(end)

fprintf('(L2,L2) Estimate \n')
Pl2l2/Pl2l2(end)

fprintf('(L2,L1) Estimate \n')
Pl2l1/Pl2l1(end)

fprintf('(L1,L1) Estimate \n')
Pl1l1/Pl1l1(end)