# Optimizing an error function involving rotation vectors in Python

I'm prototyping a system that finds the 3D pose of a object in a video sequence. For this I minimize a error function involving the rotation and translation of the object as parameters and two sets of points (polluted with gaussian noise but no outliers) as data.

I have tried to parameterize the rotation matrix both using 3 euler angles, and a rotation vector. Then I test my system with a synthetic data sequence and I see I obtain substantially less error with the 3 euler angles parameterization that with the rotation vector. I was expecting the opposite (notice the sequence I use doesn't result in any gimbal locks for the euler angles). This makes me wonder if I'm using the rotation vector parameterization correctly.

The steps I follow are, first I convert the rotation matrix to rotation vector (using the Rodrigues formula available in OpenCV) the resulting 3-vector is part of the parameters of my error function. Inside the error function I convert the rotation vector to rotation matrix and compute the residuals. I'm using python and scipy.optimize.leastsq (which internally uses MINPACK.lmdif) to minimize this error function. leastsq will compute the jacobian needed for the minimization numerically from the error function.

After reading a bit of theory about the lie group SO(3) and the lie algebra so(3), I understand that the so(3) (for me the rotation vector) corresponds to a tangent plane at the identity of SO(3). So my rotation vector is a local approximation to the corresponding rotation matrix.

What I thought that maybe I'm missing in my minimization is to convert the rotation vector back to a rotation matrix after each minimization step, and then back to a rotation vector (to take a new local approximation to the rotation matrix) for the next step. I can't test this possibility as I'm using a standard minimizer, which doesn't allow to do this.

Now I think that the Jacobian calculated at each minimization step should be enough to direct the rotation vector parameters in the correct direction without the conversions I mention in the above paragraph. However, I'm not fully sure and as I get worse performance with the rotation vector parameterization wrt. the euler angle parameterization, I assume I'm doing something wrong.

Would someone clarify/confirm the correct way of using the rotation vector in a minimization problem?

• I suggest you write the equations for the function you are trying to minimize. Then, you need to be more precise in what you mean when you say "worked most of the time" and " it kind of works but gives worse results". – Bill Greene Aug 12 '17 at 12:53
• It would be a good idea to describe what is the purpose of your project, besides adding the equations of your problem. You should also consider using scipy.optimize.minimize. – nicoguaro Aug 12 '17 at 16:22
• Ok, I've had to rephrase the whole question. I hope it's more clear now. – martinako Aug 14 '17 at 11:56
• Have you tested your code to make sure you are getting the correct solution for some simple examples, e.g. three points undergoing a simple translation or rotation? – Bill Greene Aug 14 '17 at 18:52
• Without noise it works using rotation vectors in the way I describe it, for any combination of rotation, translations. But with noise sometimes the minimization diverges, and has more error until it diverges, compared to the same code using euler angles. I wonder if the way I use the rotation vectors in the minimization is the correct way of using them, even if works in some situations. – martinako Aug 15 '17 at 12:59