Improve Randomization

[qqfly]

Manif uses random tangent space element and expMap to generate a random Lie group element:
setRandom coeffs_nonconst() = Tangent::Random().exp().coeffs()

This is not a good idea. Because uniform distribution of tangent space may not lead to uniform distribution of Lie group.

I used the following code to test the Random() function in SO(3) by generating 10000 random elements:

// generate_so3_random_elements.cpp
#include "manif/SO3.h"

#include <vector>
#include <iostream>
#include <fstream>
#include <Eigen/Dense>

using namespace manif;
using namespace std;

int main(int /*argc*/, char ** /*argv*/)
{
    ofstream file;
    file.open("SO3Randomization.txt");
    for (int i = 0; i < 10000; ++i)
    {
        SO3d X = SO3d::Random();
        file << X.coeffs().transpose() << endl;
    }
    file.close();

    return 0;
}

and plotted the axis distribution (on a unit sphere) and angle distribution (with a histogram) in Matlab:

%% plot_so3_distribution.m
clear; clc; close all;
qua = importdata('SO3Randomization.txt');
axang_arr = quat2axang(qua);
figure(1)
plot3(axang_arr(:,1), axang_arr(:,2), axang_arr(:,3), '.');
figure(2)
hist(axang_arr(:,4),100); 

The Random() function cannot generate uniformly distributed SO3 elements.

[joansola]

Hi Quiang, thanks for pointing this out and for having taken the time to produce this example.

Indeed, our sampling method is not good. We started using it in dummy tests and never improved it. We will implement a proper algorithm soon, based on http://planning.cs.uiuc.edu/node198.html.

[qqfly]

Hi Joan. I made a small mistake before. Matlab quaternion is of the form q = [w x y z], rather than q = [x y z w].
so I modified the cpp file as follows:

// generate_so3_random_elements.cpp
#include "manif/SO3.h"

#include <vector>
#include <iostream>
#include <fstream>
#include <Eigen/Dense>

using namespace manif;
using namespace std;

int main(int /*argc*/, char ** /*argv*/)
{
    ofstream file;
    file.open("SO3Randomization.txt");
    for (int i = 0; i < 20000; ++i)
    {
        SO3d X = SO3d::Random();
        file << X.w() << ", " << X.x() << ", " << X.y() << ", " << X.z() << endl;
    }
    file.close();

    return 0;
}

However, the result was still not good:

(Both axis and angle are NOT IN uniform distribution)

Besides, I tested the algorithm of Eigen::Quaterniond::unitRandom(), which is based on http://planning.cs.uiuc.edu/node198.html. This is the result:

(Axis are IN but angle are NOT IN uniform distribution)

Finally, I tested the algorithm of Sophus::SO3d::sampleUniform(), which firstly uniformly sample a point on a sphere (unit rotation axis), then uniformly sample rotation angle (but it wrap the angle to [0,pi]). This is the result:

(Both axis and angle are IN uniform distribution)

I'm not sure which would be better.

[joansola]

Hello Quiang,

I would say Sophus is doing the right thing. We have not yet investigated this issue fully.

However, in http://planning.cs.uiuc.edu/node198.html Eq. 5.15, which of the entries did you take as the real part? In my opinion it must be the last one, since it contains a cosinus. But I talk open-loop.

[qqfly]

For http://planning.cs.uiuc.edu/node198.html, the quaternion is in the form of [w, x, y, z], in other word, the first element is the real part. (I find the quaternion definition in http://planning.cs.uiuc.edu/node151.html#12337).

This is the Eigen's realization: unitRandom(). (constructor: Eigen::Quaterniond(w, x, y, z))

[joansola]

From reading this and this, the Shoemake's algorithm in http://planning.cs.uiuc.edu/node198.html seems to provide uniformly distributed x, y, z components of two randomly-rotated vectors. I tried myself and I seem to agree that the results are indeed uniform.

Are you certain on your results for this algorithm? I could not try myself yet. EDIT: I tried. You are right. However, look at my next comment.

[joansola]

The interpretation of "uniformity" in http://planning.cs.uiuc.edu/node198.html is provided by the Haar measure, which essentially determines that, if R1 and R2 are two SO3 rotations separated by an angle a(R1,R2), then this angle does not change upon modifying R1 and R2 by the same amount R, that is, a(R*R1,R*R2)=a(R1,R2).

The same can be applied to quaternions q1 and q2, with a modification by q.

(We'd have a(R1,R2) = norm(log(R1.tr * R2)) and a(q1,q2) = norm(log(q1.conj * q2)) as the respective angle functions)

Overall, I find this rather convincing.

Moreover, since Sophus deviates from this, it is not respecting the Haar measure condition.

Intuitively, it seems that once you have chosen an axis, then you have to chose an angle, and that they are unrelated. However, if you do the opposite, ie you first choose an angle and then the axis, you see that angular uniformity makes little sense: take two small angles separated by a small amount. Take two random axes. The two rotations will be separated by a small angle no matter the axes. Now take two large angles, separated by the same small amount. Take two random axes: in general, the final rotations will be separated by a large angle, since the rotations are large and the axes very different. Therefore, the distribution of angles must have low density at small angles, and this is what you observe in http://planning.cs.uiuc.edu/node198.html.

[qqfly]

You are right. Close this issue.

[joansola]

Reopen till fix arrives.

Fixing involves non-trivial work on the specialization of the Random() function.