Least-Squares Estimation of Transformation Parameters Between Two Point Sets

Motivation(s)

The following mathematical problem shows up quite often in many computer vision applications: find a similarity mapping between two sets of corresponding points \(\mathcal{X} = \left\{ \mathbf{x}_i \in \mathbb{R}^m \right\}_{i = 1}^n\) and \(\mathcal{Y} = \left\{ \mathbf{y}_i \in \mathbb{R}^m \right\}_{i = 1}^n\) that minimizes

\[\mathop{e^2}\left( \mathbf{R}, \mathbf{t}, c \right) = \frac{1}{n} \sum_{i = 1}^n \left\Vert \mathbf{y}_i - \left( c \mathbf{R} \mathbf{x}_i + \mathbf{t} \right) \right\Vert_2^2.\]

There are several solutions to this problem (a.k.a. the absolute orientation problem) when \(m\) is two or three. In particular, the most recent non-iterative algorithms is based on the SVD of the data’s covariance matrix. This solution, however, fails when the data is severely corrupted and returns a reflection instead of a rotation matrix.

Proposed Solution(s)

The author augments Arun’s least squares estimation with Lagrange multipliers to force the rotation matrix to be an orientation-preserving orthogonal matrix.

Evaluation(s)

The proposed method successfully estimates a rotation that achieves a non-zero least mean squared error instead of a reflection matrix that achieves zero error.

All of the derivations make use of the Frobenius norm:

\[\DeclareMathOperator{\tr}{tr} \left\Vert A \right\Vert_{\mathrm{F}}^2 = \tr\left( A^\top A \right).\]

According to (11), \(L'\) is symmetric so

\[L' = B A^\top R = R^\top A B^\top\]

and

\[\begin{split}{L'}^2 &= L' L'\\ &= \left( B A^\top R \right) \left( R^\top A B^\top \right)\\ &= VD U^\top U DV^\top & \quad & A B^\top = UDV^\top\\ &= V D^2 V^\top.\end{split}\]

To see that \(L'\) is commutative, notice that

\[\begin{split}L' {L'}^2 &= L' \left( L' L' \right)\\ &= \left( L' L' \right) L'\\ &= {L'}^2 L'.\end{split}\]

The foregoing properties is the reason behind (14):

\[\DeclareMathOperator{\diag}{diag} L' L' = V D^2 V^\top = \left( V D S V^\top \right) \left( V S D V^\top \right)\]

where \(S = \diag(s_i \in \{1, -1\})\).

The claim in (17) is justified because the sample covariance matrix is always positive semi-definite.

Future Direction(s)

• Would a Bayesian formulation be worthwhile?

Question(s)

• How to deal with outliers?

Analysis

This is an optimal least squares estimation of a similarity mapping between corresponding point sets. The derivation is elegant and contains a lot of useful mathematical tricks. An extension of this that weights each data point’s residual error is given in [Sor09].

#include <iostream>
#include <vector>

#include <Eigen/Dense>

int32_t main(int32_t /*argc*/, const char* /*argv*/ []) {
  // layout is x_0, y_0, x_1, x_1,...
  // this forms a DxN matrix where D is the dimension of the vector
  float pattern_x[]{0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 2.0f};
  float pattern_y[]{0.0f, 0.0f, -1.0f, 0.0f, 0.0f, 2.0f};

  auto m = 2, n = 3;
  // point correspondence already established
  Eigen::Map<const Eigen::MatrixXf> src(pattern_x, m, n);
  Eigen::Map<const Eigen::MatrixXf> dst(pattern_y, m, n);

  // homogeneous (m + 1) x (m + 1) matrix where the bottom row is e_{m + 1}
  auto result = Eigen::umeyama(src, dst, true);
  // check similarity mapping
  std::cout << result << std::endl;

  // manual inspection of each element
  std::vector<float> S(static_cast<size_t>(result.size()));
  Eigen::Map<Eigen::MatrixXf>(S.data(), result.rows(), result.cols()) = result;

  for (auto i = 0; i < result.rows(); ++i) {
    for (auto j = 0; j < result.cols(); ++j) {
      // layout is column-major
      std::cout << S[static_cast<size_t>(j * result.rows() + i)] << " ";
    }
    std::cout << std::endl;
  }

  return 0;
}

References

Sor09

Olga Sorkine. Least-squares rigid motion using svd. Technical notes, 120(3):52, 2009.

Ume91

Shinji Umeyama. Least-squares estimation of transformation parameters between two point patterns. IEEE Transactions on pattern analysis and machine intelligence, 13(4):376–380, 1991.