using matlab least squares functions

Hello,

I have my matlab code which solves a least squares problem and gives me the right answer. My code is below. I explicitly use my own analytically-derived Jacobian and so on. I just purchased the Optimization toolbox. Can anyone perhaps show me how my code can be used via the functions provided by the Optimization toolbox such as lsqnonlin and so on.

thank you.

%=========== MY least squares ==============%
clc;clear all;close all
  beep off

        X = [-0.734163292085050,-0.650030660496880;-0.734202821328435,-0.650069503240265;-0.738931528235336,-0.660060466119060;-0.737943703068185,-0.670101503002962;-0.736799998431314,-0.680143905314235;]
          Y = [-0.736371316036657,-0.661615260180661;-0.736372829883012,-0.661616774027016;-0.736552116163647,-0.662004318693837;-0.736510559472223,-0.662391863360658;-0.736462980793180,-0.662779408027478;]

          Z = X;

nit=1000
w2 =10
stopnow=false;
    w1 = 1;
    Zo = Z;
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Create kd-tree
    kd = KDTreeSearcher(Y); 
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Initialize linear system ||D^0.5(Av - b)||_2^2
      % A is the Jacobian
      % D is a weight matrix
    dim = size(Z,1)*size(Z,2);
    A = sparse(2*dim, dim+3); 
    A(1:dim,1:dim) = speye(dim,dim);
    A((1+dim):end,1:dim) = speye(dim,dim);
    A((1+dim):(dim+dim/2), end-1) = -ones(dim/2,1);
    A((1+dim+dim/2):end, end) = -ones(dim/2,1);
    b = zeros(2*dim,1)
    D = sparse(2*dim, 2*dim);
    D(1:dim,1:dim) = w1*speye(dim,dim);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    for it=1:nit
        it;
        if(stopnow) 
            return; 
        end;
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % kd-tree look-up 
        idz = knnsearch(kd,Z); 
        P = Y(idz,:);
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Build linear system
        b(1:dim) = reshape(P,dim,1);
        b((1+dim):end) = reshape(X,dim,1);
        Xr = X;
        Xr(:,1) = -Xr(:,1);
        Xr = fliplr(Xr);
        A((1+dim):end,end-2) = reshape(Xr,dim,1);
        D((dim+1):end,(dim+1):end) = w2*speye(dim,dim);
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Solve Least Squares
        v = (A'*D*A)\(A'*D*b);
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Extract solution
        Z = reshape(v(1:dim), size(X,1), size(X,2));
        theta = v(end-2);
        R = [cos(theta), -sin(theta); sin(theta) cos(theta)];
        X = X*R' + repmat(v((end-1):end)', [size(X,1),1]);
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

          %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
          % Stopping Criteria
          if(norm(Z-Zo)/size(Z,1) < 1e-6) 
              break; 
          end;
          Zo = Z;
          %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
      end

Answer 1

[Matt J]

Since your problem is simple unconstrainted linear least squares, it looks like the Optimization Toolbox would be overkill. Instead of

   v = (A'*D*A)\(A'*D*b);

however, it might be better to do

   v=lscov(A,b,D);

or

    Ds=sqrt(D);
    v=(Ds*A)\(Ds*b);

[Kate]

Matt,

I intend to extend to larger non-linear least squares and would like to know how the functions in the Optimization toolbox can be used, for example using finite of differences to compute Jacobian,etc.

Do you know how to use the Toolbox for this simple linear problem to automatically get a solution without explicitly providing jacoabian,etc ?

thank you

[Matt J]

Well lsqnonlin would be the function use, then. And by default you are not asked to supply a Jacobian. To apply it to your example in the simplest way, you would do

 Ds=sqrt(D);
 Aw=Ds*A;
 bw=Ds*b;
 v0=initial guess..

 v=lsqnonlin(@(x)Aw*x-bw, v0)

[Kate]

Matt,

A is the Jacobian in my example, so when you mention ' Aw=Ds*A' above, that means A must be provided, right?

I am having problems formulating the Objective Function for my example and feed it to lsqnonlin?

thanks again

[Matt J]

You must always provide a routine that computes the objective function.

Since your example has a linear objective, the Jacobian is conveniently available as the matrix Aw. You could therefore use

 optimoptions(@lsqnonlin,'Jacobian','on')

to tell lsqnonlin that you intend to provide the Jacobian, as described here, http://www.mathworks.com/help/optim/ug/lsqnonlin.html#f664874.

For general nonlinear objectives, though, the Jacobian isn't always conveniently available, so you do not have to use this option.

[Kate]

Matt,

I'm not sure if I explaining my problem clearly.

Essentially, I want to know: 1) from my sample code above, how do I create the objective function 2) call a matlab optimization toolbox routine to solve it.

I'm new to the Opt. Toolbox , so I was wondering if someone could have shown me how to convert my sample code to be used by it.

thanks

[Matt J]

I did show you that. In the code below (repeated from above) the objective function is fun=@(x)Aw*x-bw and the last line shows how lsqnonlin may be called to find v.

 Ds=sqrt(D);
 Aw=Ds*A;
 bw=Ds*b;
 v0=initial guess...

 v=lsqnonlin(@(x)Aw*x-bw, v0)

All of the quantities A,b,D in the code above are taken directly from your original code.

[Kate]

ok thank you matt.

[Kate]

Matt,

I have 1 more question relating to my sample code.

It solving the linear system iteratively, by chance do you know why ? since I thought this approach is reserved for non-linear problems.

cheers

[Matt J]

It looks like the code is iterating over Z. For each iterate of Z, a linear least squares sub-problem is solved in this section of the code

        % Solve Least Squares
        v = (A'*D*A)\(A'*D*b);

My presumption was that that is the least squares problem you have been talking about all this time.

It is not clear, though, what Z is supposed to represent or what the code as a whole is attempting to solve. Perhaps Z is the solution to a larger non-linear least squares problem? You have not described the purpose of the algorithm and what all the input variables mean.

[Kate]

matt,

the code is based on equation 8 of this pdf,albeit using 2D data, instead off 3D; http://lgg.epfl.ch/2d3dRegistration_data/CourseNotes.pdf

can you suggest get a more elegant way of coding equation 8 via Optimization toolbox?

I'm looking to also optimize equation 14, any help appreciated

[Matt J]

For equation (8), the easiest would be to use fmincon

optimal_RtZ =fmincon(@(RtZ)fun(RtZ,x,y,w) ,RtZ_guess,...
                        [],[],[],[],[],[],@nonlcon);

with fun() and nonlcon() implemented something like down below. You will also need a way to implement the Py() function mentioned in the paper. The following FEX file looks like it would be a good candidate

http://www.mathworks.com/matlabcentral/fileexchange/34869-distance2curve/content/distance2curve.m

For equation (14), the ideas would be very similar.

    function Ereg=fun(RtZ,x,y,w)

    %%Ensure all input matrices have the expected shape

      RtZ=RtZ(:); 
      x=reshape(x,3,[]);
      y=reshape(y,3,[]);

     %%Unpack the unknown parameters

       R=reshape(RtZ(1:9),3,3);
       t=RtZ(10:12);
       Z=reshape(RtZ(13:end),3,[]);

     %%Compute the error terms

           PyZ=... %implement with distance2curve()

       Ematch=w(1)*norm(Z-PyZ,'fro')^2;

           Rxplust=bsxfun(@plus,R*x,t);

       Eprior=w(2)*norm(Z-Rxplust, 'fro')^2;

     %%Compute total objective 

        Ereg=Ematch+Eprior;

    end

    function [c,ceq]=nonlcon(params)
    %Ensure R is a rotation matrix (orthogonal with determinant=1)

      R=reshape(params(1:9),3,3);

      v=R*R'-eye(3);

      ceq(10,1) = det(R)-1;
      ceq(1:9)= v(:);

      c=[];

    end

[Kate]

thank you Matt, Ive some questions;

1) Are these our initial guesses for the optimization?

%%Ensure all input matrices have the expected shape
      RtZ=RtZ(:); 
      x=reshape(x,3,[]);
      y=reshape(y,3,[]);
     %%Unpack the unknown parameters
       R=reshape(RtZ(1:9),3,3);
       t=RtZ(10:12);
       Z=reshape(RtZ(13:end),3,[]);

2) can we use lsqnonlin ? why use fmincon

thanks again, I'll do some testing and see how things work.

[Matt J]

1) No, all the lines of code you've quoted are from inside the objective function. They just reshape the input and separate the vector of all unknowns RtZ into separate matrices R,t, and Z. The initial guess in the code I showed you was RtZ_guess. It must be chosen by you based on your expertise on this specific registration problem ;)

2) As you can see from the lsqnonlin documentation, the only constraints it lets you specify are bound constraints. Only fmincon supports general nonlinear constraints. If you wished, you could eliminate the need for nonlinear constraints by parametrizing R in terms of Euler angles. However, the formulas for a 3D rotation matrix in terms of Euler angles are very ugly and tedious, as you can see here.

[Kate]

matt,

the approach you showed me for equation(8) works.

I made an attempt at the code for equation(13) in the PDF. Please note that for my work I am only using 3 terms in equation (13), i.e. the w1, w2 and w4 terms. I have the working, original code which is done using explicit least squares here:

http://pastebin.com/M5cd5bJd

My attempt at using your approach is here (it does not work as good as the explicit Least Squares, not sure why) :

http://pastebin.com/3d3uNZ0p

I've been trying to get My attempt (via your approach) to work for a while now , without any luck.

Please, take a look for me if you have the time and let me know if you spot anything to make an improvement.

thanks again.

[Kate]

hi matt,

did you get a chance to take a look? Please let me know.

thanks Kate

[Matt J]

Kate, I see at least 2 problems

1. You are constraining R to be a rotation matrix, but seemingly not the Ri.
2. You are calculating PyZ by discrete table lookup. The Optimization Toolbox solvers assume the objective and constraints to be continuous, twice differentiable functions of the unknowns. This will not be the case if you are using table lookup as you are. It makes PyZ a discontinuous, piece-wise constant function of Z. That is why earlier I referred you instead to the FEX file, which works by fittin a smooth continuous surface to the Y data.

[Kate]

hi Matt,

I made the updates, but still no luck. Here is my updated file: http://pastebin.com/7iuiDfuC

thank you

[Matt J]

Your constraints look the same as before.

[Kate]

hi Matt,

what do you mean, Ri is now a series of rotation matrix, as per line 41. Then I read in each Ri into the Eprior2 loop (line 72). Unless I am missing something?

thanks for helping out.

[Matt J]

The nonlcon function that I found at your link is as below. Notice that it applies constraints to R (ensuring that it is a rotation matrix) but not to Ri,

function [c,ceq]=nonlcon(params)
    %Ensure R is a rotation matrix (orthogonal with determinant=1)
      R=reshape(params(1:4),2,2);
      v=R*R'-eye(2);
      ceq(5,1) = det(R)-1;
      ceq(1:4)= v(:);
      c=[];
    end

[Kate]

matt,

I updated the 'nonlcon' function, please see it here:

http://pastebin.com/SnD57dTy

It is strange, as I am not seeing no change in the solution. Please, have a look.

thanks

[Kate]

Hi matt,

did you get an opportunity to have a look?

please let me know when you do.

cheers kate

[Matt J]

Kate, I probably won't get to it, but I recommend that you look at the exitflag and other diagnostic output arguments from fmincon to see how well the optimization succeeded. As a further test, I also recommend that you set up an ideal simulated X,Y data for which the solution Z,R,t is known and see if the objective function evaluates to 0 at the ideal solution.