27 #ifndef MLN_REGISTRATION_GET_ROT_HH
28 # define MLN_REGISTRATION_GET_ROT_HH
30 # include <mln/core/site_set/p_array.hh>
31 # include <mln/fun/x2x/all.hh>
32 # include <mln/algebra/quat.hh>
33 # include <mln/algebra/vec.hh>
34 # include <mln/math/jacobi.hh>
40 namespace registration
43 template <
typename P,
typename M>
44 fun::x2x::rotation<P::dim, float>
45 get_rot(
const p_array<P>& c,
46 const algebra::vec<P::dim,float>& mu_c,
49 const algebra::vec<P::dim,float>& mu_xk);
52 # ifndef MLN_INCLUDE_ONLY
55 template <
typename P,
typename M>
56 fun::x2x::rotation<2u, float>
57 get_rot(
const p_array<P>& c,
58 const algebra::vec<2u,float>& mu_c,
61 const algebra::vec<2u,float>& mu_xk)
85 return fun::x2x::rotation<2u, float>();
88 template <
typename P,
typename M>
89 fun::x2x::rotation<3u, float>
90 get_rot(
const p_array<P>& c,
91 const algebra::vec<3u,float>& mu_c,
94 const algebra::vec<3u,float>& mu_xk)
97 mln_precondition(3u == 3);
101 for (
unsigned i = 0; i < c.nsites(); ++i)
103 algebra::vec<3u,float> ci = convert::to< algebra::vec<3u,float> >(c[i]);
104 algebra::vec<3u,float> xki = convert::to< algebra::vec<3u,float> >(map(ck[i]));
105 Mk += (ci - mu_c) * (xki - mu_xk).t();
109 algebra::vec<3u,float> a;
110 a[0] = Mk(1,2) - Mk(2,1);
111 a[1] = Mk(2,0) - Mk(0,2);
112 a[2] = Mk(0,1) - Mk(1,0);
118 for (
int i = 1; i < 4; i++)
122 for (
int j = 1; j < 4; j++)
124 Qk(i,j) = 2 * Mk(i - 1,i - 1) - t;
127 Qk(1,2) = Mk(0,1) + Mk(1,0);
128 Qk(2,1) = Mk(0,1) + Mk(1,0);
130 Qk(1,3) = Mk(0,2) + Mk(2,0);
131 Qk(3,1) = Mk(0,2) + Mk(2,0);
133 Qk(2,3) = Mk(1,2) + Mk(2,1);
134 Qk(3,2) = Mk(1,2) + Mk(2,1);
137 qR = math::jacobi(Qk);
139 std::cout << qR << std::endl;
141 return fun::x2x::rotation<3u, float>(qR);
144 # endif // ! MLN_INCLUDE_ONLY
151 #endif // ! MLN_REGISTRATION_GET_ROT_HH