iLab Neuromorphic Robotics Toolkit  0.1
umeyamaImpl.H
1
2 #define LOCAL_EIGEN_SIZE_MIN_PREFER_DYNAMIC(a,b) ((int (a) == 0 || int (b) == 0) ? 0 \
3  : (int (a) == 1 || int (b) == 1) ? 1 \
4  : (int (a) == Eigen::Dynamic || int (b) == Eigen::Dynamic) ? Eigen::Dynamic \
5  : (int (a) <= int (b)) ? int (a) : int (b))
6
7 template <typename Derived, typename OtherDerived>
8 typename Eigen::internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type
9 nrt::umeyama (const Eigen::MatrixBase<Derived>& src, const Eigen::MatrixBase<OtherDerived>& dst, bool with_scaling)
10 {
11  typedef typename Eigen::internal::umeyama_transform_matrix_type<Derived, OtherDerived>::type TransformationMatrixType;
12  typedef typename Eigen::internal::traits<TransformationMatrixType>::Scalar Scalar;
13  typedef typename Eigen::NumTraits<Scalar>::Real RealScalar;
14  typedef typename Derived::Index Index;
15
16  EIGEN_STATIC_ASSERT (!Eigen::NumTraits<Scalar>::IsComplex, NUMERIC_TYPE_MUST_BE_REAL)
17  EIGEN_STATIC_ASSERT ((Eigen::internal::is_same<Scalar, typename Eigen::internal::traits<OtherDerived>::Scalar>::value),
18  YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
19
20  enum { Dimension = LOCAL_EIGEN_SIZE_MIN_PREFER_DYNAMIC (Derived::RowsAtCompileTime, OtherDerived::RowsAtCompileTime) };
21
22  typedef Eigen::Matrix<Scalar, Dimension, 1> VectorType;
23  typedef Eigen::Matrix<Scalar, Dimension, Dimension> MatrixType;
24  typedef typename Eigen::internal::plain_matrix_type_row_major<Derived>::type RowMajorMatrixType;
25
26  const Index m = src.rows (); // dimension
27  const Index n = src.cols (); // number of measurements
28
29  // required for demeaning ...
30  const RealScalar one_over_n = 1 / static_cast<RealScalar> (n);
31
32  // computation of mean
33  const VectorType src_mean = src.rowwise ().sum () * one_over_n;
34  const VectorType dst_mean = dst.rowwise ().sum () * one_over_n;
35
36  // demeaning of src and dst points
37  const RowMajorMatrixType src_demean = src.colwise () - src_mean;
38  const RowMajorMatrixType dst_demean = dst.colwise () - dst_mean;
39
40  // Eq. (36)-(37)
41  const Scalar src_var = src_demean.rowwise ().squaredNorm ().sum () * one_over_n;
42
43  // Eq. (38)
44  const MatrixType sigma (one_over_n * dst_demean * src_demean.transpose ());
45
46  Eigen::JacobiSVD<MatrixType> svd (sigma, Eigen::ComputeFullU | Eigen::ComputeFullV);
47
48  // Initialize the resulting transformation with an identity matrix...
49  TransformationMatrixType Rt = TransformationMatrixType::Identity (m + 1, m + 1);
50
51  // Eq. (39)
52  VectorType S = VectorType::Ones (m);
53  if (sigma.determinant () < 0)
54  S (m - 1) = -1;
55
56  // Eq. (40) and (43)
57  const VectorType& d = svd.singularValues ();
58  Index rank = 0;
59  for (Index i = 0; i < m; ++i)
60  if (!Eigen::internal::isMuchSmallerThan (d.coeff (i), d.coeff (0)))
61  ++rank;
62  if (rank == m - 1)
63  {
64  if (svd.matrixU ().determinant () * svd.matrixV ().determinant () > 0)
65  Rt.block (0, 0, m, m).noalias () = svd.matrixU () * svd.matrixV ().transpose ();
66  else
67  {
68  const Scalar s = S (m - 1);
69  S (m - 1) = -1;
70  Rt.block (0, 0, m, m).noalias () = svd.matrixU () * S.asDiagonal () * svd.matrixV ().transpose ();
71  S (m - 1) = s;
72  }
73  }
74  else
75  {
76  Rt.block (0, 0, m, m).noalias () = svd.matrixU () * S.asDiagonal () * svd.matrixV ().transpose ();
77  }
78
79  // Eq. (42)
80  if (with_scaling)
81  {
82  // Eq. (42)
83  const Scalar c = 1 / src_var * svd.singularValues ().dot (S);
84
85  // Eq. (41)
86  Rt.col (m).head (m) = dst_mean;
87  Rt.col (m).head (m).noalias () -= c * Rt.topLeftCorner (m, m) * src_mean;
88  Rt.block (0, 0, m, m) *= c;
89  }
90  else
91  {
92  Rt.col (m).head (m) = dst_mean;
93  Rt.col (m).head (m).noalias () -= Rt.topLeftCorner (m, m) * src_mean;
94  }
95
96  return (Rt);
97 }