SyB3R - Synthetic Benchmark for 3D Reconstruction
HelmertTransformation.h
1 #ifndef HELMERTTRANSFORMATION_H
2 #define HELMERTTRANSFORMATION_H
3 
4 #include <Eigen/Dense>
5 #include <stdexcept>
6 
7 namespace syb3r {
8 namespace math {
9 
10 
16 template<typename Type>
17 Eigen::Matrix<Type, 4, 4> computeHelmertTransformation(const Eigen::Matrix<Type, 3, 1> *src, const Eigen::Matrix<Type, 3, 1> *dst, unsigned count)
18 {
19  if (count < 3)
20  throw std::runtime_error("Insufficient points for helmert transformation!");
21 
22  Eigen::Matrix<Type, 3, 1> srcCenter = src[0];
23  Eigen::Matrix<Type, 3, 1> dstCenter = dst[0];
24 
25  for (unsigned i = 1; i < count; i++) {
26  srcCenter += src[i];
27  dstCenter += dst[i];
28  }
29 
30  srcCenter /= count;
31  dstCenter /= count;
32 
33  Type scale;
34  {
35  Type srcSize = Type(0);
36  Type dstSize = Type(0);
37  for (unsigned i = 0; i < count; i++) {
38  srcSize += (src[i] - srcCenter).squaredNorm();
39  dstSize += (dst[i] - dstCenter).squaredNorm();
40  }
41  scale = std::sqrt(dstSize / srcSize);
42  }
43 
44  Eigen::Matrix<Type, 3, 3> M;
45  M.setZero();
46 
47  for (unsigned i = 0; i < count; i++) {
48  Eigen::Matrix<Type, 3, 1> s = src[i] - srcCenter;
49  Eigen::Matrix<Type, 3, 1> d = dst[i] - dstCenter;
50 
51  for (unsigned k = 0; k < 3; k++)
52  for (unsigned l = 0; l < 3; l++)
53  M(k, l) += s[k] * d[l];
54  }
55 
56  Eigen::Matrix<Type, 4, 4> N;
57  N(0, 0) = M(0, 0) + M(1, 1) + M(2, 2);
58  N(0, 1) = M(1, 2) - M(2, 1);
59  N(0, 2) = M(2, 0) - M(0, 2);
60  N(0, 3) = M(0, 1) - M(1, 0);
61 
62  N(1, 0) = M(1, 2) - M(2, 1);
63  N(1, 1) = M(0, 0) - M(1, 1) - M(2, 2);
64  N(1, 2) = M(0, 1) + M(1, 0);
65  N(1, 3) = M(2, 0) + M(0, 2);
66 
67  N(2, 0) = M(2, 0) - M(0, 2);
68  N(2, 1) = M(0, 1) + M(1, 0);
69  N(2, 2) = -M(0, 0) + M(1, 1) - M(2, 2);
70  N(2, 3) = M(1, 2) + M(2, 1);
71 
72  N(3, 0) = M(0, 1) - M(1, 0);
73  N(3, 1) = M(2, 0) + M(0, 2);
74  N(3, 2) = M(1, 2) + M(2, 1);
75  N(3, 3) = -M(0, 0) - M(1, 1) + M(2, 2);
76 
77 
78  Eigen::SelfAdjointEigenSolver<decltype(N)> eigenSolver(N);
79 
80  Eigen::Quaternion<Type> rotation(
81  eigenSolver.eigenvectors()(0, 3),
82  eigenSolver.eigenvectors()(1, 3),
83  eigenSolver.eigenvectors()(2, 3),
84  eigenSolver.eigenvectors()(3, 3)
85  );
86 
87  return Eigen::Transform<Type, 3, Eigen::Affine>(Eigen::Translation<Type, 3>(dstCenter) * Eigen::Scaling(scale, scale, scale) * rotation * Eigen::Translation<Type, 3>(-srcCenter)).matrix();
88 }
89 
90 extern template Eigen::Matrix<float, 4, 4> computeHelmertTransformation<float>(const Eigen::Matrix<float, 3, 1> *src, const Eigen::Matrix<float, 3, 1> *dst, unsigned count);
91 extern template Eigen::Matrix<double, 4, 4> computeHelmertTransformation<double>(const Eigen::Matrix<double, 3, 1> *src, const Eigen::Matrix<double, 3, 1> *dst, unsigned count);
92 
93 }
94 }
95 
96 #endif // HELMERTTRANSFORMATION_H
Definition: CameraPathEvaluation.cpp:10