tak0kadaの何でもノート

発声練習、生存確認用。

医学関連は 医学ノート

球面調和関数係数で表された物体の回転推定

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));
    }
}

最後のインプットは以下のような形になっている。 f:id:kuyata:20190603063007p:plain

このプロットは以下のスクリプトで出力した。

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()