SE3 + finding the transformation matrix

[andrein...@gmail.com]

Hey, I have a question how to write an optimization procedure using manopt for the following problem.

I have data points in two different frames - one data is from the camera, I am detecting aruco markers, the second is from the kinematic chain of my robot. I want to find a transformation (translation + rotation) between those two frames. The cost function is as follow:

sum_over_all_points norm((R(alfa)R(beta)R(gamma)*P1+T-P2))^2

where alfa, beta, gamma are Euler angels
P1 - point in the one frame
P2 - point in the camera frame
T - translation between two frames
R(alfa)R(beta)R(gamma) should be a rotation from one frame to the other

My doubts are mainly concerned with:
1. I don't know where I should place boundaries (because the camera has some constraints, because of its fixed placement) so I thought that it could increase the pace of the minimalization.

2. I have also some doubts about grad of my cost function, because Rotation matrix (which is a solution of my problem) is parametrized by three variables, so how I should calculate the grad properly?

[Nicolas Boumal]

Hello,

The built-in way of dealing with rotations in R^3 in Manopt is through the manifold SO(3), which you can obtain calling rotationsfactory.

There is some documentation with a basic example here (this can also help a bit about your question regarding gradients):

http://manopt.org/manifold_documentation_rotations.html

There is also a (somewhat more involved) example using it here:

https://github.com/NicolasBoumal/manopt/blob/master/examples/generalized_procrustes.m

The important point is: rotations are here represented as 3x3 orthogonal matrices (as opposed to using Euler angles or quaternions.) Of course, it is possible to convert between these different formats, but often this is not necessary because the orthogonal matrix representation is convenient: to rotate a vector just involves a matrix-vector product.

So I think a first step is for you to write down your cost function in terms of a rotation matrix. From there, you can try to get the gradient (and we can help more easily.)

Best,

Nicolas