Optimization over the special Euclidean group SE(3)

[Nicolas Boumal]

Hello,

I received this interesting question from Yuehaw today:

Do you know whether it is possible to do joint euclidean and rotation optimization with manifold optimization? I mean a SE(3) manifold. Thanks!

Of course, the answer is yes. The special Euclidean group, or group of rigid transformations, is a manifold. One very simple way of giving it a Riemannian geometry is to consider the product SO(n) x R^n, where SO(n) is the special orthogonal group (the group of rotations) and R^n is the Euclidean space (the group of translations). Both SO(n) and R^n are readily available in Manopt, so we can use the tool productmanifold to obtain a geometry for SE(n) very quickly. It is furthermore often useful to work with multiple points on SE(n) at once, that is, work on SE(n)^k.

Code would look something like this:

SE = specialeuclideanfactory(n, k)   %  SE(n)^k for k copies of SE(n)
problem.M = SE;

problem.cost = @cost
function f = cost(X)
 % here, X.R contains the rotations and X.t contains the translations
end

problem.egrad = @egrad;
function G = egrad(X)
 % you need to return the classical (Euclidean) gradient of the cost
function here, in a structure G with fields G.R and G.t (same sizes as
in X).
end

checkgradient(problem) % always good to make sure things are going well

trustregions(problem) % and see if this works as it should :) careful: all algorithms minimize; if you need to maximize, change the signs of both cost and egrad.

As of writing this post, this is not a part of Manopt yet, but attached to this post you will find a file describing that product: specialeuclideanfactory. If you come to use it and have any comments, please let us know, so we can improve it before releasing it with the next Manopt version.

Note also that the geometry given here to SE(n) is not necessarily appropriate for all applications. Do not hesitate to comment on alternative choices / observations.

Cheers,

Nicolas

[rajendr...@iitgn.ac.in]

Hello sir,

I am solving an optimization problem on the SE(2) group. The code looks as follows:
=========
function x=se_2_main(E,X,P)

manifold = specialeuclideanfactory(2, 1);
problem.M = manifold;

problem.cost = @(M) trace(X'*[M.R M.t;zeros(1,2) 1]'*E*[M.R' -M.R'*M.t;zeros(1,2) 1]'*[M.R' -M.R'*M.t;zeros(1,2) 1]*E*[M.R M.t;zeros(1,2) 1]*X)-2*trace(P'*X'*[M.R' -M.R'*M.t;zeros(1,2) 1]*E*[M.R M.t;zeros(1,2) 1]*X);

problem.egrad = @(M) 2*E*[M.R' -M.R'*M.t;zeros(1,2) 1]'*[M.R'-M.R'*M.t;zeros(1,2) 1]*E*[M.R M.t;zeros(1,2) 1]*X*X'-2*[M.R' -M.R'*M.t;zeros(1,2) 1]'*[M.R' -M.R'*M.t;zeros(1,2) 1]*E*[M.R M.t;zeros(1,2) 1]*X*X'*[M.R M.t;zeros(1,2) 1]'*E*[M.R' -M.R'*M.t;zeros(1,2) 1]'+2*[M.R' -M.R'*M.t;zeros(1,2) 1]'*X*P*X'*[M.R M.t;zeros(1,2) 1]'*E*[M.R' -M.R'*M.t;zeros(1,2) 1]'-2*E*[M.R' -M.R'*M.t;zeros(1,2) 1]'*X*P*X';

x = trustregions(problem);
end

============
And I am getting the following error:
============
Attempt to reference field of non-structure array.

Error in productmanifold/egrad2rgrad (line 123)
g.(elems{i}) = elements.(elems{i}).egrad2rgrad(...

=============
Where, X is a 3xn matrix , P is a nxn matrix and E is 3x3 matrix. Can you please suggest a solution.

thank you

[Nicolas Boumal]

Hello,

I tried to reproduce the problem with random matrices X, P, E and n = 17, but I got a different error message.

Could you please attach .mat file with X, P, E (for a small problem)?

Best,

Nicolas

[Nicolas Boumal]

Please also copy-paste the error stack (the red messages that appear in Matlab, which show which function caused an error, and was called by which function before that, and before that..)

Thanks,

Nicolas

[Rajendra]

Hello sir,

Here is the code and the corresponding matlab error. I have attached the .mat files for the matrices P,E, and X.

thank you,

Rajendra

================Code==============

    manifold = specialeuclideanfactory(2, 1);

    problem.M = manifold;

    problem.cost = @(M) trace(X'*[M.R M.t;zeros(1,2) 1]'*E*[M.R' -M.R'*M.t;zeros(1,2) 1]'*[M.R' -M.R'*M.t;zeros(1,2) 1]*E*[M.R M.t;zeros(1,2) 1]*X)-2*trace(P'*X'*[M.R' -M.R'*M.t;zeros(1,2) 1]*E*[M.R M.t;zeros(1,2) 1]*X);

    problem.egrad = @(M) 2*E*[M.R zeros(2,1);-M.t'*M.R 1]*[M.R' -(M.R)'*M.t;zeros(1,2) 1]*E*[M.R M.t;zeros(1,2) 1]*X*X'-2*[M.R' -M.R'*M.t;zeros(1,2) 1]'*[M.R' -M.R'*M.t;zeros(1,2) 1]*E*[M.R M.t;zeros(1,2) 1]*X*X'*[M.R M.t;zeros(1,2) 1]'*E*[M.R' -M.R'*M.t;zeros(1,2) 1]'+2*[M.R' -M.R'*M.t;zeros(1,2) 1]'*X*P*X'*[M.R M.t;zeros(1,2) 1]'*E*[M.R' -M.R'*M.t;zeros(1,2) 1]'-2*E*[M.R' -M.R'*M.t;zeros(1,2) 1]'*X*P*X';

    [x, xcost, info, options] = trustregions(problem);

==============Error=================

Attempt to reference field of non-structure array.

Error in productmanifold/egrad2rgrad (line 123)

            g.(elems{i}) = elements.(elems{i}).egrad2rgrad(...

Error in getGradient (line 85)

        grad = problem.M.egrad2rgrad(x, egrad);

Error in getCostGrad (line 57)

        grad = getGradient(problem, x, storedb, key);

Error in trustregions (line 400)

[fx, fgradx] = getCostGrad(problem, x, storedb, key);

=================================================

[Nicolas Boumal]

Dear Rajendra,

I see the issue now. Just like a point M on the manifold is a structure with fields R and t, your gradient should also be a structure with fiels R and t.

Thus, problem.egrad returns a structure G such that G.R is the gradient with respect to R, and G.t is the gradient with respect to t.

Does that make sense?

Best,

Nicolas

[Rajendra]

Thank you sir,

It works after the suggested modifications.

with regards,

Rajendra

[martijnz...@gmail.com]

Dear Nicolas,

I'm a Python user interested in performing optimization on SE(3) and SE(2). Since both are yet available in pymanopt, I reviewed manopt to see how I can easily implement both manifolds the required gradients in pymanopt.

I suspect that the current implementation of SE(3) in manopt is incorrect. The root of the error lies in the fact that SE(3) is not the direct product SO(3) x R^3. Where you hinting to this when you noted "the geometry given here to SE(n) is not necessarily appropriate for all applications"?

One implication of this difference is that the exponential function for SE(3) cannot be composed of the exponential mappings of SO(3) and R^3. i.e given H an element of the lie-algebra se(3):

Exp_SE3(H) \neq [ Exp_SO3(S) & Exp_R3(v) \\ 0, 0 ]

where Exp_SE3(), Exp_SO3() and Exp_R3(), are the exponential mappings for SE(3), SO(3) and R^3, respectively.

Since SE(3) is a subset of the General Linear group, one can easily verify this by computing its exponential (inefficiently) using the matrix exponential:
Exp_SE3(H) = Exp[ S & v \\ 0, 0 ] \neq [ Exp_SO3(S) & Exp_R3(v) \\ 0, 0 ]

More efficient computations exist (see e.g.: http://ethaneade.com/lie.pdf)

Do you agree with my findings?

I'm not sure what implications it holds for the gradient computation, do you have any idea about this?

Kind regards,

Martijn Zeestraten

[Nicolas Boumal]

Hello Martijn,

Thank you for your detailed message. I do agree with your findings; this does not necessarily make the code in Manopt wrong (as I will argue), but at the very least I agree that the current documentation for the file specialeuclideanfactory is misleading and i'll change it.

You are completely right that the group product of SO(3) with R^3 to form SE(3) is not a direct product; hence, the geometry in Manopt, obtained as a product of the geometries of SO(3) and R^3 is not the Lie geometry of SE(3). It is still a "correct" geometry as far as I can tell, but it is certainly not the geometry people would expect a priori, and this is what I need to clarify in the documentation.

The reason this does not necessarily matter is that optimization is over a set: the optimization problem truly has nothing to do with the geometry we put on that set. So, in that respect, as long as we equip the set SE(3) with some geometry that Manopt can understand, then we are good to go.

This being said, picking the right geometry for a given problem can be very important, because it has direct implications for the conditioning of the problem. You can see that clearly from taking a look at the folder that contains the factories for low rank matrices: they all describe the same set (matrices of a certain size and rank), yet they all have different geometries (different metrics), and this leads to varying performance when comes the time to optimize, because the condition number of the Hessian of the cost function can vary greatly as a function of the geometry.

I hope this makes sense.

By the way, I'll keep a note of the link to the PDF file you gave about computations with the Lie structure of SE(3), because it would be good to have a "proper" implementation of that in Manopt at some point. I understand that you are a Python user, but if you get a good factory in Python for this, I'll be happy to know about it to see if we can get that in Manopt as well.

Best,

Nicolas

[martijnz...@gmail.com]

Hi Nicolas,

Thank you for the quick reply!

See my response below.

Martijn

>
> Thank you for your detailed message. I do agree with your findings; this does not necessarily make the code in Manopt wrong (as I will argue), but at the very least I agree that the current documentation for the file specialeuclideanfactory is misleading and i'll change it.
>
>

The term 'incorrect' might have been too strong: The implementation of SO(3) x R^3 is completely correct.

> You are completely right that the group product of SO(3) with R^3 to form SE(3) is not a direct product; hence, the geometry in Manopt, obtained as a product of the geometries of SO(3) and R^3 is not the Lie geometry of SE(3). It is still a "correct" geometry as far as I can tell, but it is certainly not the geometry people would expect a priori, and this is what I need to clarify in the documentation.
>
>

> The reason this does not necessarily matter is that optimization is over a set: the optimization problem truly has nothing to do with the geometry with put on that set. So, in that respect, as long as we equip the set SE(3) with some geometry that Manopt can understand, then we are good to go.

>
>
> This being said, picking the right geometry for a given problem can be very important, because it has direct implications for the conditioning of the problem. You can see that clearly from taking a look at the folder that contains the factories for low rank matrices: they all describe the same set (matrices of a certain size and rank), yet they all have different geometries (different metrics), and this leads to varying performance when comes the time to optimize, because the condition number of the Hessian of the cost function can vary greatly as a function of the geometry.
>
>
> I hope this makes sense.
>
>

I think I've got the point
* Since the sets SE(3) and SO(3) x R^3 are equal, one can perform optimization on SE(3) through SO(3) x R^3: i.e. if H is a (local) optima in SE(3) it will also be in SO(3) x R^3.
* The optimization on SO(3) x R^3 might be less efficient than the optimization on SE(3).

Even-though the optimization might yield the same result, I find the factory name SE3 misleading for the manifold SO(3) x R^3: the sets are the same but the geometry is different. As a result the exponential and logarithmic mappings are completely different. A naive user will not realize this difference.

> By the way, I'll keep a note of the link to the PDF file you gave about computations with the Lie structure of SE(3), because it would be good to have a "proper" implementation of that in Manopt at some point. I understand that you are a Python user, but if you get a good factory in Python for this, I'll be happy to know about it to see if we can get that in Manopt as well.
>

I just finished the SO(3) manifold for pymanopt, and verified that I can do optimization on SO(3) x R^3. Currently, I'm trying to implement a correct version of SE(3). However, I'm not sure how to obtain the egrad2rgrad() and ehess2rhess() functions. Do you have any hints on how to derive them?

[Nicolas Boumal]

Hello Martijn,

I actually agree about the name being misleading. It's a bit hard to change it now, since we try to be backward compatible with our releases. I will consider doing the change when we have a proper SE(n) implementation though (to be debated.) For now, you can see the latest version on the master branch has some added comments:

https://github.com/NicolasBoumal/manopt/blob/master/manopt/manifolds/specialeuclidean/specialeuclideanfactory.m

About deriving egrad2rgrad and ehess2rhess: it is going to depend on your choice of representation (how do you represent points and tangent vectors), and also on your choice of metric (inner product; which I suppose isn't much of a choice since we want the Lie geometry.) Can you tell me more about that?

(About representation of tangent vectors: the implementation in Manopt for SO(n) represents tangent vectors in the Lie algebra, that is, a tangent vector RS at R in SO(n) for some S skew-symmetric is stored simply as S. This choice presumably makes sense for SE(n) as well, although I must admit it has been the source of some confusion, because vectors in the embedding Euclidean space are still represented as RS ... we can discuss this more if need be.)

I attach the nice figure you sent me by e-mail showing the difference between geodesics in SE(2) and SO(2) x R^2. It's very clear, thanks!

Best,

Nicolas

[martijnz...@gmail.com]

Hi Nicolas,

> For now, you can see the latest version on the master branch has some added comments:
> https://github.com/NicolasBoumal/manopt/blob/master/manopt/manifolds/specialeuclidean/specialeuclideanfactory.m

Ok, I fully understand this reasoning. I hope to contribute to the proper implementation of SE(3).

>
> About deriving egrad2rgrad and ehess2rhess: it is going to depend on your choice of representation (how do you represent points and tangent vectors), and also on your choice of metric (inner product; which I suppose isn't much of a choice since we want the Lie geometry.) Can you tell me more about that?

Indeed, I would like to implement the Lie-Geometry. The inner product should in that case be the 'standard' inner-product if you represent elements of se(3)---lower case indicates lie-algebra---by 6d vectors.

> (About representation of tangent vectors: the implementation in Manopt for SO(n) represents tangent vectors in the Lie algebra, that is, a tangent vector RS at R in SO(n) for some S skew-symmetric is stored simply as S. This choice presumably makes sense for SE(n) as well, although I must admit it has been the source of some confusion, because vectors in the embedding Euclidean space are still represented as RS ... we can discuss this more if need be.)
>

The representation of tangent space elements as being embedding in the Euclidean space has also puzzled me a bit when I first looked at (py)manopt. I'm fairly new to this field, but from my understanding the key behind the intrinsic approach to Riemannian Geometry is to move away form the Euclidean embedding.

For SO(3) the tangent space actually consists of skew symmetric matrices; the matrix logarithm, logm(), of R in SO(3) is skew symmetric. This is not the case for the tangent spaces of SE(3). Here, logm(H) = [S, v; 0, 0]. This matrix is not skew-symmetric.

What is the purpose of the egrad2rgrad and ehess2rhess functions? Are they meant to 'project' a Euclidean gradient/hessian to a 'proper' tangent space element?

Cheers,
Martijn

[Nicolas Boumal]

Hello Martijn,

I understand the motivation to represent elements of the Lie algebra se(3) as vectors of length 6 (after all, this is the correct dimension), though I must say that in my experience it makes things easier for the programmer if the part in so(3) is represented as a 3x3 skew symmetric matrix (rather than just as the 3 numbers needed to define it.) Of course, this uses 12 numbers total instead of 6, which is bad for memory, but I don't expect this should be a huge problem in practice. Mostly, I'm saying this because, while for SE(2) and SE(3) in particular there are formulas one can implement directly on the 3- or 6-vector, for SE(n) in general it'll be harder to write down a formula that doesn't first reconstruct the skew symmetric matrix. If it's already formed, then that's one step not to worry about.

Going one step further, it might actually make sense to store the elements of SE(n) and se(n) as matrices of size (n+1)x(n+1). Then, the group action is naturally formed using matrix products, it's super simple to compute exp and log, etc. I actually think this makes sense.

> What is the purpose of the egrad2rgrad and ehess2rhess functions? Are they meant to 'project' a Euclidean gradient/hessian to a 'proper' tangent space element?

Yes, that is what they do. The point is to allow the user to specify the gradient at a point and the Hessian at a point along a tangent direction "as if" there were no manifold constrain. Then, these functions allow to transform those "Euclidean" derivatives into Riemannian derivatives. For M.egrad2rgrad, if M is a Riemannian submanifold of the Euclidean embedding space, this is the same as M.proj. For the Hessian it is a bit more involved. There are a lot of details in Pierre-Antoine Absil's book, chapter 5.

Cheers,

Nicolas

PS: this document also seems relevant: http://repository.upenn.edu/cgi/viewcontent.cgi?article=1155&context=meam_papers

[martijnz...@gmail.com]

Hi Nicolas,

Thanks for the additional information.
I will share the final implementation.

Martijn

[mmwi...@gmail.com]

Hello Martijn,

I was wondering what the latest status was with this project? Have you published your findings?

Thanks!

Mas