[SOLVED] Can I use Eigen Levenberg Marquardt with linear equations and constant Jacobian?

Can I use Eigen Levenberg Marquardt with linear equations and constant Jacobian?

Problem: I am trying to use eigen unsupported levenberg marquardt to find optimal parameters for two linear equations. When I run my code the algorithm converges very quickly to the incorrect answer.

Expected: I generated a bunch of terms tU, tX, tZ, tY, and tV using the parameters tFx =13.7, tFy = 13.5, tCx = 0.0, tCy = 0.0. I expect the LM to return the same values that were used to generate the tU, tX, tZ, tY, and tV.

Observed: When I run the LM on the equations it produces tFx = 13.0, tFy = 13.0, tCx = 0.0, tCy =0.0.

Approach: I am trying to find the optimal set of parameters for the following two equations:

  aH(i) = tU - tFx*tX / tZ - tCx;
  aH(i + mObjectPoints.size()) = tV - tFy*tY / tZ - tCy;

My parmeter vector is:

double tFx = aP(0);
double tFy = aP(1);
double tCx = aP(2);
double tCy = aP(3);

and aH is my function vector. The equations above produce a constant Jacobian that does not depend on aP.

  aFjac(i, 0) = -1 * tX / tZ;
  aFjac(i, 1) = 0;
  aFjac(i, 2) = -1;
  aFjac(i, 3) = 0;
  aFjac(i + mObjectPoints.size(), 0) = 0;
  aFjac(i + mObjectPoints.size(), 1) = -1*tY/tZ;
  aFjac(i + mObjectPoints.size(), 2) = 0;
  aFjac(i + mObjectPoints.size(), 3) = -1;

I am using mainly following the example shown here. As in the example I am using an operator and df that follow:

int operator()(const Eigen::VectorXd &aP,        //Input 
                     Eigen::VectorXd &aH) const  //Output 

int df(const InputType &aP, JacobianType& aFjac)

The way that I check for minimum is the following:

  Eigen::LevenbergMarquardt<CameraMatrixFunctor> lm(tFunctor);

  lm.parameters.factor = 0.001;
  lm.parameters.maxfev = 500;
  lm.parameters.xtol = 1e-5;



  lm.minimize(tP);

I left out all the initialization nonsense so that the question was clear. I would really appreciate any help since this code is unsupported. Any answers to this question will only help other people develop a better understanding of eigen's unsupported functions.

Solution

My equations were correct. If anyone else has this problem I recommend just coming up with a test case. For example in my code I just simplified my equations to

aH(i) = tFx- tCx;
aH(i + mObjectPoints.size()) = tFy - tCy;

and then the jacobian becomes

aFjac(i, 0) = 1;
aFjac(i, 1) = 0;
aFjac(i, 2) = -1;
aFjac(i, 3) = 0;
aFjac(i + mObjectPoints.size(), 0) = 0;
aFjac(i + mObjectPoints.size(), 1) = 1;
aFjac(i + mObjectPoints.size(), 2) = 0;
aFjac(i + mObjectPoints.size(), 3) = -1;

Then the LM minimizes the parameters to the correct values. Depends on what you initialize them as.