Rotation Average

What is Rotation Averaging?

Problem¹

Given a set of rotation matrices \(C_i, i=\{1,2,...,n\}\), the average of them is ? Rotation Averaging is to calculate the average of a set of rotation matrices. Averaging is to get the best estimate of all rotations. (Single rotation averaging)

Or equivalently, Rotation Averaging is the problem of estimating a set of \(n\) unknown orientations \(R_1,...,R_n \in SO(d)\) from noisy measurements \(\bar{R} \in SO(d)\) of the relative rotations \(R_i^{-1}R_j\) between them. (Multi rotation averaging)

In practical the rotation averaging problem could be categoried into : Single rotation averaging、Multi rotation averaging and Conjugate rotation averaging.

• Single rotation averaging: for instance,

used in the case where several measurements of a single rotation R are given. These may be for instance measurements of the orientation of an object, derived from measurements taken with different cameras in a calibrated network. If the measurements are noisy, they can be averaged to find a mean.

• Multi rotation averaging: for instance,

estimate the rotation of camera in SFM. More practical applications in OpenMVG.

• conjugate rotation averaging: for instance,

hand-eye coordinate problem, that is, consider a robot manipulating some object, which is also observed by a stationary camera. The orientation of the object can be computed at each moment through knowledge of the geometry of the robot (for instance, joint angles). At the same time, the orientation of the object can be computed from the images taken from the camera. This gives two separate estimates of the orientation of the object (expressed as a rotation), but these are in different coordinate frames. By solving the conjugate rotation problem, one can compute the relationship between the robot and camera frames.

For all these problems, Rotation Averaging is to find provably optimal and convergent solutions.

Rotation Averaging¹²

Assume the average rotation matrix is \(\bar{C}\), The angle \(\Delta \delta_i\) between \(\bar{C}\) and \(C_i\) , \(C_i \mbox{ and }\bar{C} \in SO(3)\), could be calculated as followings: \[ \Delta C_i=\bar{C}^TC_i \] The cosine value of \(\Delta \delta_i\) is: \[ \begin{align} cos \Delta \delta_i &= \frac{tr(\Delta C_i)-1}{2} \\ &=\frac{tr(\bar{C}^TC_i)-1}{2} \\ \end{align} \] The sum of all cosine value of \(\Delta \delta\), also known as \(Karcher\) mean: \[ \begin{align} \Sigma_i^n cos \Delta \delta_i &= \Sigma_i^n\frac{1}{2}(tr(\bar{C}^TC_i)-1) \\ &=\frac{1}{2} \Sigma_i^ntr(\bar{C}^TC_i)-\frac{n}{2} \end{align} \] Minimize the sum of angles between \(\bar{C}\) and \(C_i\) is to maximize \(\Sigma_i^ntr(\bar{C}^TC_i)\). ==??? why minimize??? to get the best estimate== \[ \begin{align} \Sigma_i^ntr(\bar{C}^TC_i) &= tr(\Sigma_i^n \bar{C}^TC_i) \\ &=tr( \bar{C}^T \Sigma_i^nC_i) \mbox{ // trace operator} \\ &=tr( \bar{C}^T U\Sigma V^T) \mbox{ // why?}\\ &=tr( V^T\bar{C}^T U\Sigma) \mbox{ // trace operator} \end{align} \] where \(V^T,U,\bar{C}^T\) are orthogonal matrices[^ 3].

Then set \(O=tr(V^T \bar{C}^T U)\)， it is also orthogonal matrix. \[ \begin{align} tr(V^T\bar{C}^T U\Sigma) &= tr(O \Sigma) \\ &= tr( \left[\begin{array}{ccc} O_{11} & O_{12} & O_{13} \\ O_{21} & O_{22} & O_{23} \\ O_{31} & O_{32} & O_{33} \end{array} \right] \left[\begin{array}{ccc} \delta_{11} & & \\ & \delta_{22} & \\ & & \delta_{33} \end{array} \right] ) \\ &= O_{11} \delta_1 + O_{22} \delta_2 + O_{33} \delta_3 \end{align} \] where \(O\) is orthogonal matrix, means \(\Sigma_{j=1}^3 O_{jk}^2=1, \mbox{ for} \quad k=1,2,3\). And the \(O_{11} \leq 1,O_{22} \leq 1,O_{33} \leq 1\).

Let \(O_{11} = 1,O_{22} = 1,O_{33} = 1\), Then \[ O=\left[\begin{array}{ccc} 1 & & \\ & 1 & \\ & & 1 \end{array} \right] =I \] then $V^T {C}^T U=I {C}^T=VUT {C} =UV^T $.

Then check the determinant of \(UV^T\), which should be 1. \[ \bar{C}=U \left[ \begin{array}{ccc} 1 & & \\ & 1 & \\ & & det(UV^T) \end{array} \right] V^T \mbox{ // why?} \]

Code

OpenMVG & Ceres for LM optimization.

Simplest toy code test:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46

// Rotation averaging in a triplet:
// 0_______2
//  \     /
//   \   /
//    \ /
//     1
TEST (rotation_averaging, RotationLeastSquare_3_Camera)
{
  //--
  // Setup 3 camera that have a relative orientation of 120 degree
  // Set Z axis as UP Vector for the rotation
  // They are in the same plane and looking in O={0,0,0}
  //--
  const Mat3 R01 = RotationAroundZ(2.*M_PI/3.0); //120 degree
  const Mat3 R12 = RotationAroundZ(2.*M_PI/3.0); //120 degree
  const Mat3 R20 = RotationAroundZ(2.*M_PI/3.0); //120 degree

  std::vector<RelativeRotation> vec_relativeRotEstimate;
  vec_relativeRotEstimate.push_back(RelativeRotation(0,1, R01));
  vec_relativeRotEstimate.push_back(RelativeRotation(1,2, R12));
  vec_relativeRotEstimate.push_back(RelativeRotation(2,0, R20));

  //- Solve the global rotation estimation problem :
  std::vector<Mat3> vec_globalR;
  // More in header file
  L2RotationAveraging(3, vec_relativeRotEstimate, vec_globalR);
  EXPECT_EQ(3, vec_globalR.size());
  // Check that the loop is closing
  EXPECT_MATRIX_NEAR(Mat3::Identity(), (vec_globalR[0]*vec_globalR[1]*vec_globalR[2]), 1e-8);

  //--
  // Check that the found relative rotation matrix give the expected rotation.
  //  -> the started relative rotation (used in the action matrix).
  //// /!\ Translations are not checked they are 0 by default.
  //--
  Mat3 R;
  Vec3 t, t0 = Vec3::Zero(), t1 = Vec3::Zero();
  RelativeCameraMotion(vec_globalR[0], t0, vec_globalR[1], t1, &R, &t);
  EXPECT_NEAR(0, FrobeniusDistance( R01, R), 1e-2);

  RelativeCameraMotion(vec_globalR[1], t0, vec_globalR[2], t1, &R, &t);
  EXPECT_NEAR(0, FrobeniusDistance( R12, R), 1e-2);

  RelativeCameraMotion(vec_globalR[2], t0, vec_globalR[0], t1, &R, &t);
  EXPECT_NEAR(0, FrobeniusDistance( R20, R), 1e-2);
}

Shonan Rotation Averaging⁴

This is a fast, simple, and elegant rotation averaging algorithm that is guaranteed to recover globally optimal solutions under mild assumptions on the measurement noise. The method employs semidefinite relaxation in order to recover provably globally optimal solutions of the rotation averaging problem.

Code

Already inside GTSAM-4.1

RCD: Rotation Coordinate Descent⁵

RCD is a fast rotation averaging algorithm that achieves global optimality under mild noise conditions on the noise level of the measurements.

Code

RCD

Hybrid Rotation Averaging: A Fast and Robust Rotation Averaging Approach⁷

Code

Graph Optimizer: This library contains not only rotation averaging solvers, but also some popular methods in 3D vision, such as translation averaging, clustering, etc. The library is designed to deal with large scale optimization problems, and is easy to extend.

iRotAvg⁶

incrementally solves rotation averaging.

Code

iRotAvg

Diff

What is the difference between rotation averaging and SLAM ? Or status estimation? SLAM tries to estimate both translation and rotation,while rotation averaging estimate the optimal rotation only.

Others

rotation averaging used by Fast Point Transformer, is a post-process optimization scheme? Perhaps yes.

Reference