Umeyama 算法求解相似变换

Kabsch–Umyama 算法是一种对齐和比较两组点之间的相似性的方法。它通过最小化点对的根平方偏差（RMSD）来找到最佳的平移，旋转和缩放。Kabsch(1976,1978) 首先描述了寻找最佳旋转的算法。Umeyama(1991) 提出了类似方法，该方法除旋转外还支持平移和缩放。

求 $c$, $\mathbf{R}$, $\mathbf{t}$ 使得下式最小:
$$
\frac{1}{n} \sum_{i=1}^n \vert\vert y_i - (c\mathbf{R}x_i + \mathbf{t}) \vert\vert_2^2
$$

注: 该问题与 Fusiello(2015) 附录 A 中提到的扩展正交普氏分析 (Extended Orthogonal Procrustes Analysis) 实际上是同一概念。

相关文献

• Kabsch, W. (1976). “A solution for the best rotation to relate two sets of vectors”. Acta Crystallographica. A32 (5): 922–923. doi:10.1107/S0567739476001873.
• Kabsch, W. (1978). “A discussion of the solution for the best rotation to relate two sets of vectors”. Acta Crystallographica. A34 (5): 827–828. doi:10.1107/S0567739478001680
• Umeyama, S. (1991). “Least-squares estimation of transformation parameters between two point patterns”. IEEE Transactions on Pattern Analysis and Machine Intelligence. 13 (4): 376–380. doi:10.1109/34.88573.
• Lawrence, J. , Bernal, J. and Witzgall, C. (2019). “A Purely Algebraic Justification of the Kabsch-Umeyama Algorithm”. Journal of Research (NIST JRES), National Institute of Standards and Technology, Gaithersburg, MD. doi:10.6028/jres.124.028
• A. Fusiello, F. Crosilla and F. Malapelle, “Procrustean Point-Line Registration and the NPnP Problem,” 2015 International Conference on 3D Vision, 2015, pp. 250-255, doi:10.1109/3DV.2015.35.

其他博客

• Aligning point patterns with Kabsch–Umeyama algorithm
• Umeyama 算法 - 微博

算法实现

• evo/evo/core/geometry.py#umeyama_alignment
• Eigen Documentation >> Eigen::umeyama()
• Eigen::umeyama 函数实现: Eigen\src\Geometry\Umeyama.h
• https://gitlab.com/libeigen/eigen/-/blob/master/Eigen/src/Geometry/Umeyama.h#L95-L164
• 调用 Eigen::umeyama(): rigid_transformation3D_srt.cpp
• 刚体变换 (不支持缩放): https://github.com/cgraumann/umeyama-matlab

代码

C++ Eigen

Eigen/src/Geometry/Umeyama.h >foldedlink

// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Hauke Heibel <hauke.heibel@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#ifndef EIGEN_UMEYAMA_H
#define EIGEN_UMEYAMA_H

// This file requires the user to include 
// * Eigen/Core
// * Eigen/LU 
// * Eigen/SVD
// * Eigen/Array

#include "./InternalHeaderCheck.h"

namespace Eigen { 

#ifndef EIGEN_PARSED_BY_DOXYGEN

// These helpers are required since it allows to use mixed types as parameters
// for the Umeyama. The problem with mixed parameters is that the return type
// cannot trivially be deduced when float and double types are mixed.
namespace internal {

// Compile time return type deduction for different MatrixBase types.
// Different means here different alignment and parameters but the same underlying
// real scalar type.
template<typename MatrixType, typename OtherMatrixType>
struct umeyama_transform_matrix_type
{
  enum {
    MinRowsAtCompileTime = internal::min_size_prefer_dynamic(MatrixType::RowsAtCompileTime, OtherMatrixType::RowsAtCompileTime),

    // When possible we want to choose some small fixed size value since the result
    // is likely to fit on the stack. So here, min_size_prefer_dynamic is not what we want.
    HomogeneousDimension = int(MinRowsAtCompileTime) == Dynamic ? Dynamic : int(MinRowsAtCompileTime)+1
  };

  typedef Matrix<typename traits<MatrixType>::Scalar,
    HomogeneousDimension,
    HomogeneousDimension,
    AutoAlign | (traits<MatrixType>::Flags & RowMajorBit ? RowMajor : ColMajor),
    HomogeneousDimension,
    HomogeneousDimension
  > type;
};

}

#endif

/**
* \geometry_module \ingroup Geometry_Module
*
* \brief Returns the transformation between two point sets.
*
* The algorithm is based on:
* "Least-squares estimation of transformation parameters between two point patterns",
* Shinji Umeyama, PAMI 1991, DOI: 10.1109/34.88573
*
* It estimates parameters \f$ c, \mathbf{R}, \f$ and \f$ \mathbf{t} \f$ such that
* \f{align*}
*   \frac{1}{n} \sum_{i=1}^n \vert\vert y_i - (c\mathbf{R}x_i + \mathbf{t}) \vert\vert_2^2
* \f}
* is minimized.
*
* The algorithm is based on the analysis of the covariance matrix
* \f$ \Sigma_{\mathbf{x}\mathbf{y}} \in \mathbb{R}^{d \times d} \f$
* of the input point sets \f$ \mathbf{x} \f$ and \f$ \mathbf{y} \f$ where 
* \f$d\f$ is corresponding to the dimension (which is typically small).
* The analysis is involving the SVD having a complexity of \f$O(d^3)\f$
* though the actual computational effort lies in the covariance
* matrix computation which has an asymptotic lower bound of \f$O(dm)\f$ when 
* the input point sets have dimension \f$d \times m\f$.
*
* Currently the method is working only for floating point matrices.
*
* \todo Should the return type of umeyama() become a Transform?
*
* \param src Source points \f$ \mathbf{x} = \left(x_1, \hdots, x_n \right) \f$.
* \param dst Destination points \f$ \mathbf{y} = \left(y_1, \hdots, y_n \right) \f$.
* \param with_scaling Sets \f$ c=1 \f$ when <code>false</code> is passed.
* \return The homogeneous transformation 
* \f{align*}
*   T = \begin{bmatrix} c\mathbf{R} & \mathbf{t} \\ \mathbf{0} & 1 \end{bmatrix}
* \f}
* minimizing the residual above. This transformation is always returned as an 
* Eigen::Matrix.
*/
template <typename Derived, typename OtherDerived>
typename internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type
umeyama(const MatrixBase<Derived>& src, const MatrixBase<OtherDerived>& dst, bool with_scaling = true)
{
  typedef typename internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type TransformationMatrixType;
  typedef typename internal::traits<TransformationMatrixType>::Scalar Scalar;
  typedef typename NumTraits<Scalar>::Real RealScalar;

  EIGEN_STATIC_ASSERT(!NumTraits<Scalar>::IsComplex, NUMERIC_TYPE_MUST_BE_REAL)
  EIGEN_STATIC_ASSERT((internal::is_same<Scalar, typename internal::traits<OtherDerived>::Scalar>::value),
    YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)

  enum { Dimension = internal::min_size_prefer_dynamic(Derived::RowsAtCompileTime, OtherDerived::RowsAtCompileTime) };

  typedef Matrix<Scalar, Dimension, 1> VectorType;
  typedef Matrix<Scalar, Dimension, Dimension> MatrixType;
  typedef typename internal::plain_matrix_type_row_major<Derived>::type RowMajorMatrixType;

  const Index m = src.rows(); // dimension
  const Index n = src.cols(); // number of measurements

  // required for demeaning ...
  const RealScalar one_over_n = RealScalar(1) / static_cast<RealScalar>(n);

  // computation of mean
  const VectorType src_mean = src.rowwise().sum() * one_over_n;
  const VectorType dst_mean = dst.rowwise().sum() * one_over_n;

  // demeaning of src and dst points
  const RowMajorMatrixType src_demean = src.colwise() - src_mean;
  const RowMajorMatrixType dst_demean = dst.colwise() - dst_mean;

  // Eq. (38)
  const MatrixType sigma = one_over_n * dst_demean * src_demean.transpose();

  JacobiSVD<MatrixType, ComputeFullU | ComputeFullV> svd(sigma);

  // Initialize the resulting transformation with an identity matrix...
  TransformationMatrixType Rt = TransformationMatrixType::Identity(m+1,m+1);

  // Eq. (39)
  VectorType S = VectorType::Ones(m);

  if  (svd.matrixU().determinant() * svd.matrixV().determinant() < 0 )
    S(m-1) = -1;

  // Eq. (40) and (43)
  Rt.block(0,0,m,m).noalias() = svd.matrixU() * S.asDiagonal() * svd.matrixV().transpose();

  if (with_scaling)
  {
    // Eq. (36)-(37)
    const Scalar src_var = src_demean.rowwise().squaredNorm().sum() * one_over_n;

    // Eq. (42)
    const Scalar c = Scalar(1)/src_var * svd.singularValues().dot(S);

    // Eq. (41)
    Rt.col(m).head(m) = dst_mean;
    Rt.col(m).head(m).noalias() -= c*Rt.topLeftCorner(m,m)*src_mean;
    Rt.block(0,0,m,m) *= c;
  }
  else
  {
    Rt.col(m).head(m) = dst_mean;
    Rt.col(m).head(m).noalias() -= Rt.topLeftCorner(m,m)*src_mean;
  }

  return Rt;
}

} // end namespace Eigen

#endif // EIGEN_UMEYAMA_H

C++ OpenCV

estimateRigidTransform3D_srt()>folded

// 计算多个三维点对之间的最优相似变换矩阵 
// estimate R,t s.t. a*R*src + t = dst
// src,dst 为 n 行 3 列矩阵, 每行都是一个点的三维坐标 
// 如果 scale<=0, 则使用给定的数据估计缩放因子, 否则使用传入的实参作为缩放因子;
// https://www.cnblogs.com/wqj1212/p/3915859.html
// https://github.com/opencv/opencv/blob/478663b08c2050546d06b4d3586386954343f80b/modules/video/include/opencv2/video/tracking.hpp#L258
// https://github.com/opencv/opencv/blob/828304d587b3a204ebc34ce8573c3adec5a41ad2/modules/video/src/lkpyramid.cpp#L1508
double estimateRigidTransform3D_srt(const cv::Mat& src, const cv::Mat& dst, double& scale, cv::Mat& R, cv::Mat& t)
{
	CV_Assert(src.rows == dst.rows);
	CV_Assert((src.cols == 3) && (dst.cols == 3));
	int n = src.rows;
	cv::Mat e = cv::Mat::ones(n, 1, CV_64F);
	cv::Mat II = cv::Mat::eye(n, n, CV_64F) - cv::Mat::ones(n, n, CV_64F) / n;
	cv::Mat U, Sigma, Vt;
	cv::SVD::compute(dst.t() * II * src, Sigma, U, Vt);
	cv::Mat Sigma_det = cv::Mat::eye(3, 3, CV_64F);
	Sigma_det.at<double>(2, 2) = cv::determinant(U * Vt);
	R = U * Sigma_det * Vt;
	cv::Mat AR = dst * R;
	if (scale <= 0)
	{
		scale = cv::trace(dst.t() * II * dst).val[0] / cv::trace(AR.t() * II * src).val[0];
	}
	cv::Mat c_trans;
	cv::reduce(scale * src - AR, c_trans, 0, CV_REDUCE_AVG);// 逐列求均值 
	cv::Mat c = c_trans.t();
	t = -R * c;
	cv::Mat Y = scale * src - dst * R - e * c_trans;
	double err = cv::norm(Y) / sqrt(n);
	return err;
}

Python

umeyama.py >foldedlink

# umeyama function from scikit-image/skimage/transform/_geometric.py

import numpy as np

def umeyama( src, dst, estimate_scale ):
    """Estimate N-D similarity transformation with or without scaling.
    Parameters
    ----------
    src : (M, N) array
        Source coordinates.
    dst : (M, N) array
        Destination coordinates.
    estimate_scale : bool
        Whether to estimate scaling factor.
    Returns
    -------
    T : (N + 1, N + 1)
        The homogeneous similarity transformation matrix. The matrix contains
        NaN values only if the problem is not well-conditioned.
    References
    ----------
    .. [1] "Least-squares estimation of transformation parameters between two
            point patterns", Shinji Umeyama, PAMI 1991, DOI: 10.1109/34.88573
    """

    num = src.shape[0]
    dim = src.shape[1]

    # Compute mean of src and dst.
    src_mean = src.mean(axis=0)
    dst_mean = dst.mean(axis=0)

    # Subtract mean from src and dst.
    src_demean = src - src_mean
    dst_demean = dst - dst_mean

    # Eq. (38).
    A = np.dot(dst_demean.T, src_demean) / num

    # Eq. (39).
    d = np.ones((dim,), dtype=np.double)
    if np.linalg.det(A) < 0:
        d[dim - 1] = -1

    T = np.eye(dim + 1, dtype=np.double)

    U, S, V = np.linalg.svd(A)

    # Eq. (40) and (43).
    rank = np.linalg.matrix_rank(A)
    if rank == 0:
        return np.nan * T
    elif rank == dim - 1:
        if np.linalg.det(U) * np.linalg.det(V) > 0:
            T[:dim, :dim] = np.dot(U, V)
        else:
            s = d[dim - 1]
            d[dim - 1] = -1
            T[:dim, :dim] = np.dot(U, np.dot(np.diag(d), V))
            d[dim - 1] = s
    else:
        T[:dim, :dim] = np.dot(U, np.dot(np.diag(d), V.T))

    if estimate_scale:
        # Eq. (41) and (42).
        scale = 1.0 / src_demean.var(axis=0).sum() * np.dot(S, d)
    else:
        scale = 1.0

    T[:dim, dim] = dst_mean - scale * np.dot(T[:dim, :dim], src_mean.T)
    T[:dim, :dim] *= scale

    return T

Matlab

estimateRigidTransform3D_srt.m >folded

% init
clc;clear;close all;
%% Test 坐标系 A 坐标系 B
point_A = [0, -2.14185, -1.1497
    0,-1.6855,-1.1068
    -0.24,-1.6905,-1.0969
    0,-0.473,-1.146]
point_B = [-0.9121385,-0.5697752,1.051605
    -0.4756734,-0.5859144,1.152362
    -0.5229784,-0.6114097,1.401835
    0.6978357,-0.5654929,1.69446]

[n,~] = size(point_A)

[R,t] = estimateRigidTransform3D_srt(point_A, point_B)
error = R*point_A.' + t*ones(1,n) - point_B.'
err = vecnorm(error)

figure(1);
S_transformed = R*point_A.' + t*ones(1,n);
S_transformed = S_transformed.';
plot3(point_A(:,1),point_A(:,2),point_A(:,3),'bd','MarkerSize',5);hold on;
plot3(point_B(:,1),point_B(:,2),point_B(:,3),'bd','MarkerSize',5);hold on;
plot3(S_transformed(:,1),S_transformed(:,2),S_transformed(:,3),'b*','MarkerSize',5);hold on;
grid on;axis square;

%% implement
function [R, t, a] = estimateRigidTransform3D_srt(Src, Dst)
% same usage as gPPnP
unit_scale = (nargout <3);
n = size(Src,1);
e = ones(n,1); II = eye(n)-((e*e')./n);
[U,~,V] = svd((Dst)'*II*Src);
R=U*[1 0 0; 0 1 0; 0 0 det(U*V')]*V';
A = Dst; AR=A*R;
if unit_scale
    a=1;
else
    a = trace((A)'*II*(A))/trace(AR'*II*Src);
end
c = mean((a*Src-AR),1)';
t = -R*c;
end