돛단배

bool CDisplayRelativeCalibration::ComputeCameraCalibration(void)
{
    CTsai tsai;

    tsai.Width(m_camerawid);
    tsai.Height(m_camerahit);

    for(std::list<Corr>::const_iterator i=m_cameracorr.begin();  i!=m_cameracorr.end();  i++)
    {
        tsai.Point(i->x, i->y, i->z, i->u, i->v);
    }

    if(tsai.PointCount() < 8)
        return Error("Didn't get enough points");

    if(!tsai.Compute())
        return Error("Camera calibration failed");

    for(int n=0;  n<tsai.PointCount();  n++)
    {
        double ux, uy;
        tsai.WorldToImage (tsai.PointX(n), tsai.PointY(n), tsai.PointZ(n), ux, uy);

        m_cameraproj.push_back(CGrPoint(ux, uy, 0));
    }

   
    m_cameraf = tsai.F();
    m_cameracx = tsai.Cx();
    m_cameracy = tsai.Cy();
    m_camerakappa1 = tsai.Kappa1();
    m_camerasx = tsai.Sx();
    memcpy(m_cameramatrix, tsai.CameraMatrix(), sizeof(double) * 16);

    return true;
}

class CTsai
{

    bool ncc_compute_exact_f_and_Tz();
    bool ncc_compute_exact_f_and_Tz_error (int m_ptr, int n_ptr, const double *params, double *err);

};

bool CTsai::ncc_compute_exact_f_and_Tz()
{
    CLmdif<CTsai> lmdif;

    lmdif.Lmdif (this, ncc_compute_exact_f_and_Tz_error,
            m_point_count, NPARAMS, x,
            NULL, NULL, NULL, NULL);
}

template<class T> class CLmdif : private CLmdif_
{

note: candidates are: int CLmdif<T>::Lmdif(T*, bool (T::*)(int, int, const double*, double*), int, int, double*, double*, double*, int*, double*) [with T = CTsai]

error: no matching function for call to 'CLmdif<CTsai>::Lmdif(CTsai* const, bool (*)(int, int, const double*, double*), int&, const int&, double [3], NULL, NULL, NULL, NULL)'

error: no matching function for call to 'CLmdif<CTsai>::Lmdif(CTsai* const, bool (&)(int, int, const double*, double*), int&, const int&, double [3], NULL, NULL, NULL, NULL)'

           if ( CRimage.size() > 0 ) // if there is a valid point with its cross ratio
            {  
                correspondPoints(indexI, indexW, p, CRimage, linesYorder.size(), linesXorder.size(), world, CRworld, dxList.size(), dyList.size(), iplMatch, scale );
            }  
            cvShowImage( "match", iplMatch );
            cvSaveImage( "match.bmp", iplMatch );
           
            cout << "# of pairs = " << indexI.size() << " = " << indexW.size() << endl;
           
            // # 6. camera calibration
           
            int numPair = indexI.size();
           
            tsai.Clear();
           
            for( int n = 0;  n < numPair;  n++ )
            {
                tsai.Point(world[indexW[n]].x, world[indexW[n]].y, world[indexW[n]].z, p[indexI[n]].x, p[indexI[n]].y);
               
                cout << "pair #" << n << ": " << p[indexI[n]].x << "  " <<  p[indexI[n]].y << "  : "
                    << world[indexW[n]].x << "  " << world[indexW[n]].y << "  " << world[indexW[n]].z << endl;
            }
           
            if( numPair < 8 )
                cout << "Didn't get enough points" << endl;
           
            if(!tsai.Compute())
                cout << "Camera calibration failed" << endl;
           
            cout << endl << "camera parameter" << endl
            << "focus = " << tsai.F() << endl
            << "principal axis (x,y) = " <<  tsai.Cx() << ", " <<  tsai.Cy() << endl
            << "kappa1 (lens distortion) = " <<  tsai.Kappa1() << endl
            << "skew_x = " << tsai.Sx() << endl;
          
            // reproject world points on to the image frame to check the result of computing camera parameters
            for(int n=0;  n<tsai.PointCount();  n++)
            {
                double ux, uy;
                tsai.WorldToImage (tsai.PointX(n), tsai.PointY(n), tsai.PointZ(n), ux, uy);
                CvPoint reproj = cvPoint( cvRound(ux), cvRound(uy) );
                cvCircle( iplInput, reproj, 3, CV_RGB(200,100,200), 2 );
            }
           

# of pairs = 16 = 16
pair #0: 7.81919  36.7864  : 119.45  82.8966  0
pair #1: 15.1452  71.2526  : 119.45  108.484  0
pair #2: 26.1296  122.93  : 119.45  147.129  0
pair #3: 36.6362  172.36  : 119.45  182.066  0
pair #4: 77.3832  20.4703  : 159.45  82.8966  0
pair #5: 85.4293  53.7288  : 159.45  108.484  0
pair #6: 97.8451  105.05  : 159.45  147.129  0
pair #7: 109.473  153.115  : 159.45  182.066  0
pair #8: 96.6046  15.962  : 171.309  82.8966  0
pair #9: 105.046  48.8378  : 171.309  108.484  0
pair #10: 118.177  99.9803  : 171.309  147.129  0
pair #11: 130.4  147.586  : 171.309  182.066  0
pair #12: 145.469  4.50092  : 199.965  82.8966  0
pair #13: 154.186  36.5857  : 199.965  108.484  0
pair #14: 168.033  87.5497  : 199.965  147.129  0
pair #15: 180.732  134.288  : 199.965  182.066  0

# of found lines = 8 vertical, 7 horizontal
vertical lines:
horizontal lines:
p.size = 56
CRimage.size = 56

# of pairs = 20 = 20
pair #0: -42.2331  53.2782  : 102.07  108.484  0
pair #1: -22.6307  104.882  : 102.07  147.129  0
pair #2: -4.14939  153.534  : 102.07  182.066  0
pair #3: 1.81771  169.243  : 102.07  193.937  0
pair #4: -10.9062  41.1273  : 119.45  108.484  0
pair #5: 8.69616  92.7309  : 119.45  147.129  0
pair #6: 27.0108  140.945  : 119.45  182.066  0
pair #7: 32.9779  156.653  : 119.45  193.937  0
pair #8: 57.4164  14.6267  : 159.45  108.484  0
pair #9: 77.7374  65.9516  : 159.45  147.129  0
pair #10: 96.3391  112.934  : 159.45  182.066  0
pair #11: 102.524  128.555  : 159.45  193.937  0
pair #12: 76.5236  7.21549  : 171.309  108.484  0
pair #13: 97.5633  58.2616  : 171.309  147.129  0
pair #14: 116.706  104.705  : 171.309  182.066  0
pair #15: 123.108  120.238  : 171.309  193.937  0
pair #16: 125.015  -11.5931  : 199.965  108.484  0
pair #17: 146.055  39.453  : 199.965  147.129  0
pair #18: 164.921  85.2254  : 199.965  182.066  0
pair #19: 171.323  100.758  : 199.965  193.937  0

camera parameter
focus = 3724.66
principal axis (x,y) = 168.216, 66.5731
kappa1 (lens distortion) = -6.19473e-07
skew_x = 1

Urban mapping and construction of 3D urban models has become a popular area of research recently.  The object is to construct models from images taken by a vehicle moving in the urban environment. Typically such a vehicle will take images on both sides simultaneously, with cameras pointing in opposite directions.  Part of the problem is to estimate the motion of the vehicle from a sequence of images taken by the non-overlapping cameras.  I will discuss methods of doing this task, leading to three different algorithms that we have developed for estimating the motion of a rig with non-overlapping cameras.

>> c = [-120; -100; 0; 0];
>> A = [ 2, 2, 1, 0; 5, 3, 0, 1];
>> b = [8, 15];
>> x= sedumi(A,b,c)
SeDuMi 1.1R3 by AdvOL, 2006 and Jos F. Sturm, 1998-2003.
Alg = 2: xz-corrector, Adaptive Step-Differentiation, theta = 0.250, beta = 0.500
eqs m = 2, order n = 5, dim = 5, blocks = 1
nnz(A) = 6 + 0, nnz(ADA) = 4, nnz(L) = 3
 it :     b*y       gap    delta  rate   t/tP*  t/tD*   feas cg cg  prec
  0 :            4.18E-002 0.000
  1 : -5.02E+002 1.25E-002 0.000 0.2989 0.9000 0.9000   1.77  1  1  3.8E+000
  2 : -4.29E+002 3.65E-003 0.000 0.2927 0.9000 0.9000   2.52  1  1  6.4E-001
  3 : -4.29E+002 2.84E-004 0.000 0.0778 0.9900 0.9900   1.19  1  1  4.5E-002
  4 : -4.30E+002 9.99E-007 0.000 0.0035 0.9990 0.9990   1.03  1  1 
iter seconds digits       c*x               b*y
  4      0.2   Inf -4.3000000000e+002 -4.3000000000e+002
|Ax-b| =  2.3e-015, [Ay-c]_+ =  0.0E+000, |x|= 2.9e+000, |y|= 3.6e+001

Detailed timing (sec)
   Pre          IPM          Post
3.125E-002    1.563E-001    0.000E+000   
Max-norms: ||b||=15, ||c|| = 120,
Cholesky |add|=0, |skip| = 0, ||L.L|| = 2.11427.
x =
   (1,1)       1.5000
   (2,1)       2.5000
>>

>> c = [5; 7; 9; 6; 7; 10; 0; 0; 0; 0; 0];
>> A = [1,1,1,0,0,0,1,0,0,0,0; 0,0,0,1,1,1,0,1,0,0,0; 1,0,0,1,0,0,0,0,-1,0,0; 0,1,0,0,1,0,0,0,0,-1,0; 0,0,1,0,0,1,0,0,0,0,-1];
>> b = [120; 140; 100; 60; 80];
>> x = sedumi(A,b,c)
SeDuMi 1.1R3 by AdvOL, 2006 and Jos F. Sturm, 1998-2003.
Alg = 2: xz-corrector, Adaptive Step-Differentiation, theta = 0.250, beta = 0.500
eqs m = 5, order n = 12, dim = 12, blocks = 1
nnz(A) = 17 + 0, nnz(ADA) = 17, nnz(L) = 12
 it :     b*y       gap    delta  rate   t/tP*  t/tD*   feas cg cg  prec
  0 :            3.24E+003 0.000
  1 :  1.16E+003 9.69E+002 0.000 0.2991 0.9000 0.9000   2.37  1  1  1.6E+000
  2 :  1.63E+003 2.05E+002 0.000 0.2114 0.9000 0.9000   1.34  1  1  3.0E-001
  3 :  1.69E+003 3.84E+001 0.000 0.1875 0.9000 0.9000   1.20  1  1  5.1E-002
  4 :  1.70E+003 1.08E+000 0.000 0.0282 0.9900 0.9900   1.03  1  1 
iter seconds digits       c*x               b*y
  4      0.5  15.9  1.7000000000e+003  1.7000000000e+003
|Ax-b| =  5.6e-014, [Ay-c]_+ =  7.6E-016, |x|= 1.1e+002, |y|= 1.4e+001

Detailed timing (sec)
   Pre          IPM          Post
3.125E-001    4.531E-001    1.250E-001   
Max-norms: ||b||=140, ||c|| = 10,
Cholesky |add|=0, |skip| = 0, ||L.L|| = 1.
x =
   (1,1)      68.6083
   (3,1)      51.3917
   (4,1)      31.3917
   (5,1)      60.0000
   (6,1)      28.6083
   (8,1)      20.0000

>> help sedumi
                                           [x,y,info] = sedumi(A,b,c,K,pars)
   
SEDUMI  Self-Dual-Minimization/ Optimization over self-dual homogeneous
          cones.
 
  >  X = SEDUMI(A,b,c) yields an optimal solution to the linear program
       MINIMIZE c'*x SUCH THAT A*x = b, x >= 0
       x is a vector of decision variables.
       If size(A,2)==length(b), then it solves the linear program
       MINIMIZE c'*x SUCH THAT A'*x = b, x >= 0
 
  >  [X,Y,INFO] = SEDUMI(A,b,c) also yields a vector of dual multipliers Y,
       and a structure INFO, with the fields INFO.pinf, INFO.dinf and
       INFO.numerr.
 
     (1) INFO.pinf=INFO.dinf=0: x is an optimal solution (as above)
       and y certifies optimality, viz.\ b'*y = c'*x and c - A'*y >= 0.
       Stated otherwise, y is an optimal solution to
       MAXIMIZE b'*y SUCH THAT c-A'*y >= 0.
       If size(A,2)==length(b), then y solves the linear program
       MAXIMIZE b'*y SUCH THAT c-A*y >= 0.
 
     (2) INFO.pinf=1: there cannot be x>=0 with A*x=b, and this is certified
       by y, viz. b'*y > 0 and A'*y <= 0. Thus y is a Farkas solution.
 
     (3) INFO.dinf=1: there cannot be y such that c-A'*y >= 0, and this is
       certified by x, viz. c'*x <0, A*x = 0, x >= 0. Thus x is a Farkas
       solution.
 
     (I)   INFO.numerr = 0: desired accuracy achieved (see PARS.eps).
     (II)  INFO.numerr = 1: numerical problems warning. Results are accurate
           merely to the level of PARS.bigeps.
     (III) INFO.numerr = 2: complete failure due to numerical problems.
 
     INFO.feasratio is the final value of the feasibility indicator. This
     indicator converges to 1 for problems with a complementary solution, and
     to -1 for strongly infeasible problems. If feasratio in somewhere in
     between, the problem may be nasty (e.g. the optimum is not attained),
     if the problem is NOT purely linear (see below). Otherwise, the reason
     must lie in numerical problems: try to rescale the problem.
 
  >  [X,Y,INFO] = SEDUMI(A,b,0) or SEDUMI(A,b) solves the feasibility problem
     FIND x>=0 such that A*x = b
 
  >  [X,Y,INFO] = SEDUMI(A,0,c) or SEDUMI(A,c) solves the feasibility problem
     FIND y such that A'*y <= c
 
  >  [X,Y,INFO] = SEDUMI(A,b,c,K) instead of the constraint "x>=0", this
       restricts x to a self-dual homogeneous cone that you describe in the
       structure K. Up to 5 fields can be used, called K.f, K.l, K.q, K.r and
       K.s, for Free, Linear, Quadratic, Rotated quadratic and Semi-definite.
       In addition, there are fields K.xcomplex, K.scomplex and K.ycomplex
       for complex-variables.
 
     (1) K.f is the number of FREE, i.e. UNRESTRICTED primal components.
       The dual components are restricted to be zero. E.g. if
       K.f = 2 then x(1:2) is unrestricted, and z(1:2)=0.
       These are ALWAYS the first components in x.
 
     (2) K.l is the number of NONNEGATIVE components. E.g. if K.f=2, K.l=8
       then x(3:10) >=0.
 
     (3) K.q lists the dimensions of LORENTZ (quadratic, second-order cone)
       constraints. E.g. if K.l=10 and K.q = [3 7] then
           x(11) >= norm(x(12:13)),
           x(14) >= norm(x(15:20)).
       These components ALWAYS immediately follow the K.l nonnegative ones.
       If the entries in A and/or c are COMPLEX, then the x-components in
       "norm(x(#,#))" take complex-values, whenever that is beneficial.
        Use K.ycomplex to impose constraints on the imaginary part of A*x.
 
     (4) K.r lists the dimensions of Rotated LORENTZ
       constraints. E.g. if K.l=10, K.q = [3 7] and K.r = [4 6], then
           2*x(21)x(22) >= norm(x(23:24))^2,
           2*x(25)x(26) >= norm(x(27:30))^2.
       These components ALWAYS immediately follow the K.q ones.
       Just as for the K.q-variables, the variables in "norm(x(#,#))" are
       allowed to be complex, if you provide complex data. Use K.ycomplex
       to impose constraints on the imaginary part of A*x.
 
     (5) K.s lists the dimensions of POSITIVE SEMI-DEFINITE (PSD) constraints
       E.g. if K.l=10, K.q = [3 7] and K.s = [4 3], then
           mat( x(21:36),4 ) is PSD,
           mat( x(37:45),3 ) is PSD.
       These components are ALWAYS the last entries in x.
 
     (a) K.xcomplex lists the components in f,l,q,r blocks that are allowed
      to have nonzero imaginary part in the primal. For f,l blocks, these
     (b) K.scomplex lists the PSD blocks that are Hermitian rather than
       real symmetric.
     (c) Use K.ycomplex to impose constraints on the imaginary part of A*x.
 
     The dual multipliers y have analogous meaning as in the "x>=0" case,
     except that instead of "c-A'*y>=0" resp. "-A'*y>=0", one should read that
     c-A'*y resp. -A'*y are in the cone that is described by K.l, K.q and K.s.
     In the above example, if z = c-A'*y and mat(z(21:36),4) is not symmetric/
     Hermitian, then positive semi-definiteness reflects the symmetric/
     Hermitian parts, i.e. Z + Z' is PSD.
 
     If the model contains COMPLEX data, then you may provide a list
     K.ycomplex, with the following meaning:
       y(i) is complex if ismember(i,K.ycomplex)
       y(i) is real otherwise
     The equality constraints in the primal are then as follows:
           A(i,:)*x = b(i)      if imag(b(i)) ~= 0 or ismember(i,K.ycomplex)
           real(A(i,:)*x) = b(i)  otherwise.
     Thus, equality constraints on both the real and imaginary part
     of A(i,:)*x should be listed in the field K.ycomplex.
 
     You may use EIGK(x,K) and EIGK(c-A'*y,K) to check that x and c-A'*y
     are in the cone K.
 
  >  [X,Y,INFO] = SEDUMI(A,b,c,K,pars) allows you to override the default
       parameter settings, using fields in the structure `pars'.
 
     (1) pars.fid     By default, fid=1. If fid=0, then SeDuMi runs quietly,
       i.e. no screen output. In general, output is written to a device or
       file whose handle is fid. Use fopen to assign a fid to a file.
 
     (2) pars.alg     By default, alg=2. If alg=0, then a first-order wide
       region algorithm is used, not recommended. If alg=1, then SeDuMi uses
       the centering-predictor-corrector algorithm with v-linearization.
       If alg=2, then xz-linearization is used in the corrector, similar
       to Mehrotra's algorithm. The wide-region centering-predictor-corrector
       algorithm was proposed in Chapter 7 of
         J.F. Sturm, Primal-Dual Interior Point Approach to Semidefinite Pro-
         gramming, TIR 156, Thesis Publishers Amsterdam, 1997.
 
     (3) pars.theta, pars.beta   By default, theta=0.25 and beta=0.5. These
       are the wide region and neighborhood parameters. Valid choices are
       0 < theta <= 1 and 0 < beta < 1. Setting theta=1 restricts the iterates
       to follow the central path in an N_2(beta)-neighborhood.
 
     (4) pars.stepdif, pars.w. By default, stepdif = 2 and w=[1 1].
        This implements an adaptive heuristic to control ste-differentiation.
        You can enable primal/dual step length differentiation by setting stepdif=1 or 0.
       If so, it weights the rel. primal, dual and gap residuals as
       w(1):w(2):1 in order to find the optimal step differentiation.
 
     (5) pars.eps     The desired accuracy. Setting pars.eps=0 lets SeDuMi run
       as long as it can make progress. By default eps=1e-8.
 
     (6) pars.bigeps  In case the desired accuracy pars.eps cannot be achieved,
      the solution is tagged as info.numerr=1 if it is accurate to pars.bigeps,
      otherwise it yields info.numerr=2.
 
     (7) pars.maxiter Maximum number of iterations, before termination.
 
     (8) pars.stopat  SeDuMi enters debugging mode at the iterations specified in this vector.
 
     (9) pars.cg      Various parameters for controling the Preconditioned conjugate
      gradient method (CG), which is only used if results from Cholesky are inaccurate.
     (a) cg.maxiter   Maximum number of CG-iterates (per solve). Theoretically needed
           is |add|+2*|skip|, the number of added and skipped pivots in Cholesky.
           (Default 49.)
     (b) cg.restol    Terminates if residual is a "cg.restol" fraction of duality gap.
           Should be smaller than 1 in order to make progress (default 5E-3).
     (c) cg.refine    Number of refinement loops that are allowed. The maximum number
           of actual CG-steps will thus be 1+(1+cg.refine)*cg.maxiter. (default 1)
     (d) cg.stagtol  Terminates if relative function progress less than stagtol (5E-14).
     (e) cg.qprec    Stores cg-iterates in quadruple precision if qprec=1 (default 0).
 
     (10) pars.chol   Various parameters for controling the Cholesky solve.
      Subfields of the structure pars.chol are:
     (a) chol.canceltol: Rel. tolerance for detecting cancelation during Cholesky (1E-12)
     (b) chol.maxu:   Adds to diagonal if max(abs(L(:,j))) > chol.maxu otherwise (5E5).
     (c) chol.abstol: Skips pivots falling below abstol (1e-20).
     (d) chol.maxuden: pivots in dense-column factorization so that these factors
       satisfy max(abs(Lk)) <= maxuden (default 5E2).
 
     (11) pars.vplot  If this field is 1, then SeDuMi produces a fancy
       v-plot, for research purposes. Default: vplot = 0.
 
     (12) pars.errors  If this field is 1 then SeDuMi outputs some error
     measures as defined in the Seventh DIMACS Challenge. For more details
     see the User Guide.
 
  Bug reports can be submitted at http://sedumi.mcmaster.ca.
 
  See also mat, vec, cellK, eyeK, eigK

* MATLAB에서 그래프 그리기

ezplot('x*log(x)')
로 해서 나왔으나, 일반적으로는 다음과 같은 식으로 한다.

x=0:0.1:10;
y=x.*log2(x);  ## 여기서, x 다음의 .은 element끼리의 scalar 연산을 지시함
plot(x,y)