A Robust Method for Rotation Estimation Using Spherical Harmonics Representation - IEEE Journals & Magazineを実装した。以下はコードを貼り付けたのみ。
実装
#include <vector> #include <array> #include <cassert> #include <numeric> #include <eigen3/Eigen/Core> #include <eigen3/Eigen/Eigenvalues> #include <iostream> // #include <x86intrin.h> using quaternion = std::array<double, 4>; using matrix = std::array<std::vector<double>, 3>; template <typename matrix> std::vector<int> dim(const matrix&); template <> std::vector<int> dim(const std::array<std::vector<double>, 3>& m) { return {3, static_cast<int>(m[0].size())}; } // Althloothi, Salah, Mohammad H. Mahoor, and Richard M. Voyles. // "A robust method for rotation estimation using spherical harmonics representation." // IEEE Transactions on Image Processing 22.6 (2013): 2306-2316. // // align two objects represented by 3xN matrix, whose i-th column is a vector for sphere harmonics coefficients template <typename quaternion, typename matrix> quaternion align(const matrix& from, const matrix& to) { #ifdef DEBUG assert(dim(from)[0] == 3); assert(dim(to)[0] == 3); assert(dim(from)[1] == dim(to)[1]); #endif const int N{dim(from)[1]}; // p2311 (18) // Covariance matrix of obj and target Eigen::MatrixXd Cov(3,3); for (int i = 0; i < 3; ++i) { for (int j = 0; j < 3; ++j) { double c{0}; for (int n = 0; n < N; ++n) { c += from[i][n] * to[j][n]; } Cov(i, j) = c; } } //p.2311 (19) Eigen::MatrixXd Q(4, 4); Q(0,0) = Cov(0,0) + Cov(1,1) + Cov(2,2); Q(0,1) = Cov(1,2) - Cov(2,1); Q(0,2) = Cov(2,0) - Cov(0,2); Q(0,3) = Cov(0,1) - Cov(1,0); Q(1,0) = Q(0,1); Q(1,1) = Cov(0,0) - Cov(1,1) - Cov(2,2); Q(1,2) = Cov(0,1) + Cov(1,0); Q(1,3) = Cov(2,0) + Cov(0,2); Q(2,0) = Q(0,2); Q(2,1) = Q(1,2); Q(2,2) = -Cov(0,0) + Cov(1,1) - Cov(2,2); Q(2,3) = Cov(1,2) + Cov(2,1); Q(3,0) = Q(0,3); Q(3,1) = Q(1,3); Q(3,2) = Q(2,3); Q(3,3) = -Cov(0,0) - Cov(1,1) + Cov(2,2); // Eigen decomposition Eigen::EigenSolver<Eigen::MatrixXd> solver{Q}; const auto val = solver.eigenvalues(); const auto vec = solver.eigenvectors(); // find the largest eigen value and correspoinding eigen vector // quarternion to rotate obj is the eigenvector corresponds to the largest eigenvalue std::size_t idx{0}; // type of val(idx) is std::complex // we don't need imaginary part because we know the element is real value double max = val(idx).real(); for (int i = 0; i < 4; ++i) { if (val(i).real() > max) { max = val(idx).real(); idx = i; } } quaternion ret{ // Eigen matrix is column-wise by default vec.col(idx)(0).real(), vec.col(idx)(1).real(), vec.col(idx)(2).real(), vec.col(idx)(3).real()}; return ret; }
テスト
#define BOOST_TEST_MAIN #include <boost/test/included/unit_test.hpp> #include <boost/test/tools/detail/tolerance_manip.hpp> // #include <iostream> // #include <iomanip> // for std::setprecision #include <cmath> #include "althloothi.hpp" const double pi{std::acos(-1)}; using quaternion = std::array<double, 4>; quaternion operator- (const quaternion& rot) { return quaternion{-rot[0], -rot[1], -rot[2], -rot[3]}; } // template <typename T, std::size_t N> // std::ostream& operator<< (std::ostream &ios, const std::array<T, N> &arr) { // ios << "{"; // for (int i = 0; i < N - 1; ++i) // ios << arr[i] << ", "; // ios << arr[N-1] << "}"; // // return ios; // } // // template <typename T> // std::ostream& operator<< (std::ostream &ios, const std::vector<T> &vec) { // int n = vec.size(); // ios << "{"; // for (int i = 0; i < n - 1; ++i) // ios << vec[i] << ", "; // ios << vec[n-1] << "}"; // // return ios; // } #define CHECK_CLOSE_COLLECTION(a_vec, b_vec, tolerance) { \ assert(a_vec.size() == b_vec.size()); \ for (std::size_t i = 0; i < a_vec.size(); ++i) { \ BOOST_TEST(a_vec[i] == b_vec[i], tolerance); \ } \ } BOOST_AUTO_TEST_CASE(test_align) { { // spheres std::array<std::vector<double>, 3> Sh1 = {{{0, 0, 0, 1}, {0, 1, 0, 0}, {0, 0, 1, 0}}}; std::array<std::vector<double>, 3> Sh0 = {{{0, 0, 0, 1}, {0, 1, 0, 0}, {0, 0, 1, 0}}}; auto qvec = align<quaternion>(Sh1, Sh0); // no rotation CHECK_CLOSE_COLLECTION(qvec, quaternion({1, 0, 0, 0}), boost::test_tools::tolerance(1e-12)); } { // ellipsoids std::array<std::vector<double>, 3> Sh1 = {{{0, 0, 0, 2}, {0, 1, 0, 0}, {0, 0, 1, 0}}}; std::array<std::vector<double>, 3> Sh0 = {{{0, -1, 0, 0}, {0, 0, 0, 2}, {0, 0, 1, 0}}}; auto qvec = align<quaternion>(Sh1, Sh0); // rotation around z-axis auto rot = quaternion{std::cos(pi/4), 0, 0, std::sin(pi/4)}; if (qvec[0] * rot[0] < 0) rot = -rot; CHECK_CLOSE_COLLECTION(qvec, rot, boost::test_tools::tolerance(1e-12)); } { // ellipsoids std::array<std::vector<double>, 3> Sh1 = {{{0, 0, 0, 2}, {0, 1, 0, 0}, {0, 0, 1, 0}}}; std::array<std::vector<double>, 3> Sh0 = {{{0, 0, 1, 0}, {0, 1, 0, 0}, {0, 0, 0, 2}}}; auto qvec = align<quaternion>(Sh1, Sh0); // rotation around y-axis auto rot = quaternion({std::cos(-pi/4), 0, std::sin(-pi/4), 0}); if (qvec[0] * rot[0] < 0) rot = -rot; CHECK_CLOSE_COLLECTION(qvec, rot, boost::test_tools::tolerance(1e-12)); } { std::array<std::vector<double>, 3> Sh1 = {{{0, 0, 0, 1}, {0, 2, 0, 0}, {0, 0, 4, 0}}}; std::array<std::vector<double>, 3> Sh0 = {{{0, 0, 4, 0}, {0, 0, 0, 1}, {0, 2, 0, 0}}}; auto qvec = align<quaternion>(Sh1, Sh0); auto rot = quaternion{std::cos(pi/3), std::sin(pi/3)/std::sqrt(3), std::sin(pi/3)/std::sqrt(3), std::sin(pi/3)/std::sqrt(3)}; if (qvec[0] * rot[0] < 0) rot = -rot; CHECK_CLOSE_COLLECTION(qvec, rot, boost::test_tools::tolerance(1e-12)); } { // some wavy shape std::array<std::vector<double>, 3> Sh1 = {{{0.,0.,0.,1.,0.5,0.}, {0.,2.,0.,0.,1.,1.}, {0.,0.,3.,0.,1.,0.}}}; std::array<std::vector<double>, 3> Sh0 = {{{0.,0.,3.,0.,1.,0.}, {0.,0.,0.,1.,0.5,0.}, {0.,2.,0.,0.,1.,1.}}}; auto qvec = align<quaternion>(Sh1, Sh0); auto rot = quaternion{std::cos(pi/3), std::sin(pi/3)/std::sqrt(3), std::sin(pi/3)/std::sqrt(3), std::sin(pi/3)/std::sqrt(3)}; if (qvec[0] * rot[0] < 0) rot = -rot; CHECK_CLOSE_COLLECTION(qvec, rot, boost::test_tools::tolerance(1e-12)); } }
最後のインプットは以下のような形になっている。
このプロットは以下のスクリプトで出力した。
coef_x = np.array([0,0,0,1,0.5,0]) coef_y = np.array([0,2,0,0,1,1]) coef_z = np.array([0,0,3,0,1,0]) n_div = 30 theta = [i/n_div*np.pi for i in range(n_div)] phi = [i/n_div*np.pi*2 for i in range(n_div)] def sph_harm(l, m, theta, phi): if m > 0: return np.sqrt(2) * (-1)**m * _sph_harm(m, l, phi, theta).real if m == 0: return _sph_harm(m, l, phi, theta).real else: return np.sqrt(2) * (-1)**m * _sph_harm(-m, l, phi, theta).imag def l(n): return int(np.sqrt(n)) def m(n): l_ = l(n) return n - l_**2 - l_ def x(theta, phi): return np.sum([coef_x[n] * sph_harm(l(n), m(n), theta, phi) for n in range(coef_x.size)]) def y(theta, phi): return np.sum([coef_y[n] * sph_harm(l(n), m(n), theta, phi) for n in range(coef_y.size)]) def z(theta, phi): return np.sum([coef_z[n] * sph_harm(l(n), m(n), theta, phi) for n in range(coef_z.size)]) X = []; Y = []; Z = [] for t in theta: for p in phi: X.append(x(t, p)) Y.append(y(t, p)) Z.append(z(t, p)) fig = plt.figure() ax = Axes3D(fig) ax.scatter(X, Y, Z) plt.show()