1 #ifndef HELMERTTRANSFORMATION_H 2 #define HELMERTTRANSFORMATION_H 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)
20 throw std::runtime_error(
"Insufficient points for helmert transformation!");
22 Eigen::Matrix<Type, 3, 1> srcCenter = src[0];
23 Eigen::Matrix<Type, 3, 1> dstCenter = dst[0];
25 for (
unsigned i = 1; i < count; i++) {
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();
41 scale = std::sqrt(dstSize / srcSize);
44 Eigen::Matrix<Type, 3, 3> M;
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;
51 for (
unsigned k = 0; k < 3; k++)
52 for (
unsigned l = 0; l < 3; l++)
53 M(k, l) += s[k] * d[l];
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);
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);
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);
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);
78 Eigen::SelfAdjointEigenSolver<decltype(N)> eigenSolver(N);
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)
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();
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);
96 #endif // HELMERTTRANSFORMATION_H Definition: CameraPathEvaluation.cpp:10