Inequivalence between SE(3) and <R3, SO(3)>

[LucasWaelti]

I have been playing around with the library and was testing how to average poses. In the code below, I do so using both the $SE(3)$ group and the composition $\langle\mathbb{R}^3, SO(3)\rangle$.

While the average angle is computed well with both groups, the average position is less precise when using the $SE(3)$ group.

See the code below that reproduces the results:

#include <Eigen/Dense>
#include <manif/manif.h>

#include "utils/geometry/quaternion.hpp" // for roll, pitch, yaw to quaternion
#include "utils/sampling.hpp" // for normal distribution sampling

// Define the groups to use for the tests
using R3            = manif::R3f;
using R3Tangent     = manif::R3Tangentf;
using SO3           = manif::SO3f;
using SO3Tangent    = manif::SO3Tangentf;
using SE3           = manif::SE3f;
using SE3Tangent    = manif::SE3Tangentf;
using R3SO3         = manif::Bundle<float, manif::R3, manif::SO3>;
using R3SO3Tangent  = R3SO3::Tangent;

void SE3_average(const std::vector<SE3>& M){
    SE3 avg_lie = SE3::Identity();
    SE3Tangent avg_lie_t = avg_lie.log();
    for(auto m : M){
        SE3Tangent m_t = m.log();
        avg_lie_t += m_t;
    }
    avg_lie_t /= M.size();
    avg_lie = avg_lie_t.exp();

    std::cout << "[SE3] Avg pose:\n" << avg_lie.coeffs().transpose() << std::endl;
}

void R3SO3_average(const std::vector<SE3>& M){
    R3SO3 avg_lie = R3SO3::Identity();
    R3SO3Tangent avg_lie_t = avg_lie.log();
    for(SE3 m : M){
        R3SO3 mb(m); // convert the group to the bundle
        avg_lie_t += mb.log();
    }
    avg_lie_t /= M.size();
    avg_lie = avg_lie_t.exp();

    std::cout << "[R3SO3] Avg pose:\n" << avg_lie.coeffs().transpose() << std::endl;
}

int main(){

    SE3 pose_ref = SE3::Identity();
    std::cout << "[True] Pose reference:\n" << pose_ref.coeffs().transpose() << std::endl;

    // List of poses to compute the average from 
    std::vector<SE3> M;

    // Build poses around the identity at each corner of a cube
    for(int i=0; i<2; i++)
        for (int x=-1; x<2; x++)
            for(int y=-1; y<2; y++)
                for(int z=-1; z<2; z++){
                    
                    // Set the reference of the angles at 0 or 2*pi
                    float l = i%2==0 ? 0.0f : 2.0f*M_PI;

                    SE3 m(
                        Eigen::Vector3f(x, y, z), 
                        // Set roll, pitch and yaw angles and convert them to a quaternion with a normal distribution
                        geo::to_quaternion(normal(l,0.2),normal(l,0.2),normal(l,0.2))
                    );

                    M.push_back(m);
                }

    // Compute the average pose using different methods
    SE3_average(M);
    R3SO3_average(M);

    return 0;
}

with the output:

[True] Pose reference:
0 0 0 0 0 0 1
[SE3] Avg pose:
 -0.159439  -0.149208   0.217482 0.00391522  0.0271461 -0.0141559   0.999524
[R3SO3] Avg pose:
         0          0          0 0.00391522  0.0271461 -0.0141559   0.999524

Where is this discrepancy coming from? Happy to provide additional details if relevant.

[joansola]

Hi Lucas.

The tangent spaces of SE3 and <R3,SO3> are not the same. If you average on these spaces, the result will not match.

In <R3,SO3>, the translation part does not interact with the rotation, so the position average should be just the arithmetic mean of all the input positions.

In SE3, the mapping to the tangent space affects the position with the orientation, in two ways. One is by rotatinng it with the rotaiton matrix R(theta). the other one is by affecting it with the matrix V(theta). See the micro-Lie paper, exp and log maps of SE3 for reference.

Given these differences, you should not expect these two averagings to give the same result.

[joansola]

Also, you should NEVER use Euler angles to do anything related to orientations other than user interface. The random values you input for the quaternion are Gaussian in the Euler space, but this does not mean you get a well behaved distribution of quaternions around the identity. In your case it "works" because you limit sigma to 0,2 rad, which is "small".

You can use some randomization directly in quaternion space. Eigen has Quaternion::Random(), and I think manif also has the Random() method for all groups, which has been well tested.

[joansola]

Another comment, is that rather than averaging in the tangent space at the identity, as you do, you should average at the tangent space of the averaged element. This means that you-d better construct an iterative algorithm that computes an average, and using this average, recomputes a second average around this one, and iterates a few times.

[joansola]

In any case, the differences between SE3 and <R3,SO3> will still appear I think.

[joansola]

Another comment, is that rather than averaging in the tangent space at the identity, as you do, you should average at the tangent space of the averaged element. This means that you-d better construct an iterative algorithm that computes an average, and using this average, recomputes a second average around this one, and iterates a few times.

You can achieve this with something like the following:

X_ave0 = exp(sum_{n=0}^N (log(X_n)) / N)             // initial average around the identity
for j in 1..10
{
   X_ave_j = X_ave_{j-1} * exp( sum_{n=0}^N ( log( X_ave_{j-1}.inv * X_n )) / N )          // average around the previous average X_ave_{j-1}
}

At the end of the iterations, the term inside the exponential should have converged to zero, meaning that all elements are equally spread around the average -- which is what you want. You can use this fact to define a stopping criterion for the iterations.

[LucasWaelti]

Hi Joan,

Thank you very much for the very detailed (and fast!) answer. Very helpful.

I am still a bit confused about why the results should differ for both cases. I would expect the Log and Exp maps to compensate for these differences between the SE(3) and R3SO(3) groups since they describe the same thing (although they indeed differ significantly in their definitions)...

But my understanding of Lie theory is still a bit limited, so I do not want to take more of your time. I greatly appreciate if you can comment on this still, but otherwise feel free to close this issue :)

[joansola]

You can look at Example 7 in the paper, it touches on the differences between SE3 and R3xSO3 and <R3,SO3>

As per the little algorithm, you can see it better using (+) and (-), so you can implement them easily in manif:

// initial average around the identity
X_ave0 = exp(sum_{n=0}^N (X_n (-) Identity) / N)    
         
for j in 1..100
{
   // average around the previous average X_ave_{j-1}
   X_ave_j = X_ave_{j-1} (+) sum_{n=0}^N ( X_n (-) X_ave_{j-1})) / N )     

   // exit condition
   if norm(X_ave_j (-) X_ave_{j-1})  < 1e-8
      return
}

See that this is resembling the averages you would do to regular vectors, using plain + and -:

// initial average around the identity ( Identity = 0 for vectors under regular sum)
x_ave0 = sum_{n=0}^N (x_n - 0) / N)       
      
for j in 1..10
{
   // average around the previous average x_ave_{j-1}
   x_ave_j = x_ave_{j-1} + sum_{n=0}^N ( x_n - x_ave_{j-1})) / N )     
}

but in the case of vectors, the iterations are doing nothing because the first average is just right. the others do nothing. In case of Lie groups, the iterations progressively converge to the best unbiased average.

[artivis]

This matter of averaging Lie Groups is a rabbit hole I did not had time to explore. However, I poked a little a it back in the days and the algorithm @joansola is describing is implemented in manif/algorithms/average.h. Mind that it's only 'proper' for compact groups, which have a bi-invariant Riemannian metric .
There is another algorithm implemented in the file link above (left/right Frechet mean) that's valid for rigid transformations given that the dispersion of the data is 'small'.