Optimization For Computer Vision, Having Trouble Optimizing a Specific Function Found in Literature

The last line of this post has a very short version of this question with no extra detail.

I am trying to implement an alternate calibration technique described in Geometric Calibration for Mobile, Stereo, Autofocus Cameras. While I have made some attempts, I am still not confident that I am optimizing the function correctly.

More specifically,

I have an intrinsic matrix for each camera defined by Kc = [f 0 u;0 f v;0 0 1] where f is focal length in pixels and (u,v) is the optical center for each camera. The letter c is a subscript denoting which camera each variable belongs to. Additionally, I have a rotation Rc for each camera (this is the same as Rrectl and Rrectr inside the rectifyStereoImages function). Finally, these parameters define a homography, Hc, used to rectify each image in the pair where Hc = K*Rc*inv(Kc) and Rc and Kc are rotations and intrinsics of the specific camera. K is defined as the average of the intrinsics for each camera (e.g. K = 0.5*(K0+K1)). A rectified point qc in this case is defined by qc = Hc*Pc where Pc is a non-rectified point (x y 1)'.

The above is described by equations 5 through 9 in the reference paper.

The goal of this technique is to minimize the difference in rectified Y coordinates of corresponding points. Additionally, some regularization terms are added (equation 12 and 15).

Overall, we are trying to implement equation 14 and we are hoping to optimize parameters derived from a matlab calibration using the online technique described in this paper.

Now some information regarding our attempts so far.

we have successfully (at least to our best knowledge) extracted the parameters Hc, Rc, Kc, and K as described by the paper from a set of matlab stereo parameters. For more information on how we accomplished this, see the MatLab function 'computeRectificationParameters' (it is inside StereoParametersImpl.m you can get there by editing 'rectifyStereoImages.m' and from there opening 'rectifyStereoImagesImpl.m').

Inside you can see the relations Hleft = projective2d((K_new * Rrectl / Kl)'); and Hright = projective2d((K_new * Rrectr / Kr)'); which follows the same relation described by eq 8 in the paper. From here we extracted the appropriate parameters.

In order to optimize these parameters, we created a script to first match features and compute the fundamental matrix to remove outlier correspondence. To save space I will not include our code for this portion since it is a fairly straight forward process.

Now using the set of inlier correspondences along with the parameters from our initial calibration we attempt to optimize the following function:

    function output = estimateRightValsOnlineMod(unknowns,ptsLeft,ptsRight,...
        uleft,vleft,lambdaF,lambdaR,rleftOffline,rrightOffline)

        global Cf; global CR;

        %Get unknown variables out
        Rinfo.fx = unknowns(1);
        Rinfo.u = unknowns(2);
        Rinfo.v = unknowns(3);
        rleft = [unknowns(4),unknowns(5),unknowns(6)];
        rright = [unknowns(7),unknowns(8),unknowns(9)];
        Linfo.fx = unknowns(10);

        Rleft = rotationVectorToMatrix(rleft);
        Rright = rotationVectorToMatrix(rright);

        %Build Ks
        Linfo.u = uleft;
        Linfo.v = vleft;
        [Kleft,Kright,Kaverage] = buildKs(Linfo,Rinfo);

        %Get homography
        Hleft = Kaverage*Rleft*inv(Kleft);
        Hright = Kaverage*Rright*inv(Kright);

        %Apply homographies to points
        qleft = Hleft*[ptsLeft(:,1)';ptsLeft(:,2)';ones(length(ptsLeft(:,1)),1)'];
        qright = Hright*[ptsRight(:,1)';ptsRight(:,2)';ones(length(ptsRight(:,1)),1)'];

        %Get the difference in y values
        delta = qleft(2,:) - qright(2,:);

        %Calculate our regularized focal length and rotation
        Cf = lambdaF*(Linfo.fx-Rinfo.fx).^2;
        CR = lambdaR*(norm(rleft-rleftOffline).^2+norm(rright-rrightOffline).^2);

        %Run the final formula
        output = double(sum(delta.^2) + Cf + CR);
      end

      function [Kleft,Kright,Kaverage] = buildKs(linfo,rinfo)

          Kleft = [linfo.fx,0,linfo.u;
                   0,linfo.fx,linfo.v;
                   0,0,1];
          Kright = [rinfo.fx,0,rinfo.u;
                    0,rinfo.fx,rinfo.v;
                    0,0,1];
          Kaverage = 0.5*(Kleft + Kright);
      end

Now inside our main function after we determine inlier correspondences. k0,k1,R0,R1 are the parameters extracted from the initial MatLab parameter object as described above. Assume inlierPoints1 and inlierPoints2 contain a set of about 4000 matching points:

    %Get values from k0
    fleftx = k0(1,1);
    uleft = k0(1,3);
    vleft = k0(2,3);

    %Set initial guess for 
    frightx = k1(1,1);
    uright = k1(1,3);
    vright = k1(2,3);

    %Create Rodrigues Vector from Rotation Matrices
    rleft = rotationMatrixToVector(R0);
    rright = rotationMatrixToVector(R1);

   %Create vector of values we want to optimize (x0 in documentation for optimization)
   unknownsOL = [frightx,uright,vright,rleft(1),rleft(2),rleft(3),rright(1),rright(2),rright(3),fleftx];

   %Create anonymous function and send the function to fminunc solver
   opts = optimoptions(@fminunc,'Display','iter-detailed','Diagnostic','on',...
   'StepTolerance',0.0000000000000000000000000001,...
   'FunctionTolerance',0.00000000001);

   lambdaF = 10;
   lambdaRO = 1;
   fn = @(unknownsOL)estimateRightValsOnlineMod(unknownsOL,inlierPoints1,inlierPoints2,...
    uleft,vleft,lambdaF,lambdaRO,rleftOffline,rrightOffline);
   [unknownValsOnline,~,valOnline,~,oot] = fminunc(fn,unknownsOL,opts);

Overall our optimization output results are not as good as the original inputs (we say this based on the quality of the rectified images). The rectification is near perfect using the original input parameters, while point correspondences differ in y by anywhere from 1 to 10 using the parameters of the optimized output. We are not sure if we are using the right solver, the right options, or even that the built-in matlab functions are capable of optimizing such a function.

The area of implementing the optimization is specifically where we are looking for any guidance.

I realize that my explanation may not be the best as there is a lot of information to include and I did not want to make the post too long. However, in my opinion what is most important is that the reader understands the equations from the original paper and is comfortable with the optimization toolbox.

Any help or advice regarding the optimization process would be greatly appreciated. I would be happy to clarify anything that is vague or unclear. Thank you.

The short version of my question is how would you implement equation 14 in the linked paper using the optimization toolbox?