ceres-solver backend

[Andrew Hundt]

http://ceres-solver.org/ now supports constraints. How practical do you think a Tasks backend would be?

[Hervé Audren]

I am not sure what to mean by "practical" but I can help: I see that Ceres is typically used as a non-linear least squares, large-scale problem solver. I think that using it for our (relatively) small scale, linear least squares problems is probably not the best application. However, if it manages to perform better than LSSOL it would be great!

Now, how to do this? You need to add two files, CeresQPSolver.{cpp,h} in which you define:

• The constructor, to hold all the necessary data
• updateSize() will be called every time the size of the problem changes. This only happens when the number of constraints changes. It happens for example when changing contacts, and when triggering the joint limits activation. You thus need to resize accordingly your inner data structures.
• updateMatrix() will give you the list of current tasks (objectives) and constraints.
• solve() should only solve the problem.
• result() should return the vector of variables.

Note that there will often be a lot of updateMatrix and solve() calls in between updateSize() calls, thus you should do all your allocation and expensive transformations at that time.

I do not know Ceres very well, but here are my thoughts:

• In your constructor, you probably need to create a Problem. You should also set the solver options here. As we are linear, one iteration of Ceres should be enough, with the trust region size set to maximum. This corresponds to saying that the second-order approximation is valid everywhere.
• Your cost function should return something like:

double* residual = new double[totalTaskSize];
for(const auto task : tasks)
{
    for(int i = curIndex; i < task.dim(); ++i)
    {
        residual[i] = task->weight * task->eval();
     }
     curIndex += task.dim();
}

• Then, you will need to compose in a similar way the jacobians to return the jacobian of the problem, that will roughly be the statcked, weighted jacobians (you can get it from task->jac()).
• Finally, if you can, your problem should return the (constant) hessian of the problem.
• The constraints should be handled similarly, but don't forget that we have various types of constraints:
  • Equality : A x = b
  • Inequality : A x <= u
  • GenInequality i.e. l <= A x <= u

Note that we are still thinking about how to make Tasks more easily expansible, so if you have any remarks/suggestions, feel free to ask :) I think that you might run into trouble here, as we usually manage directly Q and c matrices instead of relying on the actual jacobians of the linear forms.

For example, when manipulating SetPointTasks you will notice that:

Q = J^T * dimW * J
C = -J^T*W*(kp*eval() - kv*speed() - normalAcc)

For ceres to work, I think you have to return:

error = kp*eval() - kv*speed() - normalAcc
jac = J

[Pierre Gergondet]

Few additional notes:

1. CeresQPSolver should derive from GenQPSolver
2. You should edit the solver map in src/GenQPSolver.cpp (line 55), something like {"Ceres", allocateQP<CeresQPSolver>}
3. You can then use the Ceres solver by calling solver("Ceres") on your QPSolver instance
4. Alternatively, you can also change the default solver in src/GenQPSolver.cpp (l. 41) to Ceres. This will also let the unit tests run using this solver.

[Andrew Hundt]

Wow, what great explanations! Sorry, I was just evaluating the possibility. Perhaps these two comments should be put in a doc explaining how to add a new backend?

I had thought of asking this in the past but decided against it at the time, but now there is constrained optimization and specific examples where it is used for a variety of robotics algorithms http://ceres-solver.org/nnls_tutorial.html#other-examples.

[Hervé Audren]

Yes we should probably add those remarks to the wiki... I'll see to it Monday.

For Ceres, unfortunately I'm not sure it really supports constraints: what I gather from a quick look at the slam examples is that they are approximating the actual constraints by a minimisation of the error. The only constraints they seem to enforce are bounds constraints, that are not sufficient for our needs. If you can point me in the documentation where you can actually ask to enforce Ax <= b, that would be great.

Note : it is also possible to approximate constraints by non linear cost functions, but this would require multiple iterations.

[Andrew Hundt]

http://ceres-solver.org/nnls_modeling.html

Ceres solves robustified bounds constrained non-linear least squares problems of the form:
[...constrained least squares equation...]

Don't know how to link mathjax so just follow the link.

Oh haha yeah Ax <= b... that's quite an important part... everything there is just constraints on vectors like L <= x <= U, not matrices and a vector, whoops!

[Adrien Escande]

Well, it would be possible to solve problems with general inequality constraints:
First transform

  min. f
  s.t. l<= Ax <=u

into (with I, the identity matrix)

  min. f
  s.t. Ax - Is = 0
       l <= s <= u

Note that in this second formulation, s is also a variable.
Second, use the Ceres LocalParameterization to implement +(x,\delta x) = x + P\delta x where P is the projector onto the nullspace of [A -I] (typically P = Z Z^T for Z a basis of the nullspace, obtained by QR decomposition of [A -I]^T).
You can then solve

  min. f
  s.t. l <= s <= u

with this parameterization, starting from a solution of Ax = s (if Ceres requires the initial point to be within the bounds, it might not be trivial to find such a point, but it is feasible, by minimizing ||Ax-s||^2 over the same bounds).
However, this solution should not be very competitive from a computation time point of view.

Note that this approach extends trivially to encompass linear equality constraints.

[Andrew Hundt]

Yeah convex problems of the form we solve here can even be represented as unconstrained minimization, I guess performance is the main issue.

I bet it is worth asking on the Ceres mailing list if there are plans for the type of inequalities we would need or what kind of performance could be expected, in case there is some internal detail that may be more informative than the API alone. I put in to join it.

[Andrew Hundt]

Posted the question:
https://groups.google.com/d/topic/ceres-solver/IIm6msf8jcI/discussion

[Andrew Hundt]

They replied quickly, it isn't supported. Author said he wants to implement it eventually, which means it could be quite a while. Perhaps I should close this issue? Should some of your notes be added to the readme.md for adding a new backend?

[Hervé Audren]

Yes I think we can now close this issue, I added some notes here, feel free to contribute :)