00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #ifndef MLN_REGISTRATION_GET_ROT_HH
00027 # define MLN_REGISTRATION_GET_ROT_HH
00028
00029 # include <mln/core/site_set/p_array.hh>
00030 # include <mln/fun/x2x/all.hh>
00031 # include <mln/algebra/quat.hh>
00032 # include <mln/algebra/vec.hh>
00033 # include <mln/math/jacobi.hh>
00034
00035
00036 namespace mln
00037 {
00038
00039 namespace registration
00040 {
00041
00042 template <typename P, typename M>
00043 fun::x2x::rotation<P::dim, float>
00044 get_rot(const p_array<P>& c,
00045 const algebra::vec<P::dim,float>& mu_c,
00046 const p_array<P>& ck,
00047 const M& map,
00048 const algebra::vec<P::dim,float>& mu_xk);
00049
00050
00051 # ifndef MLN_INCLUDE_ONLY
00052
00053
00054 template <typename P, typename M>
00055 fun::x2x::rotation<2u, float>
00056 get_rot(const p_array<P>& c,
00057 const algebra::vec<2u,float>& mu_c,
00058 const p_array<P>& ck,
00059 const M& map,
00060 const algebra::vec<2u,float>& mu_xk)
00061 {
00062 assert(0 && "TODO");
00063
00065
00067
00068
00069
00071
00072
00073
00075
00076
00077
00078 return fun::x2x::rotation<2u, float>();
00079 }
00080
00081 template <typename P, typename M>
00082 fun::x2x::rotation<3u, float>
00083 get_rot(const p_array<P>& c,
00084 const algebra::vec<3u,float>& mu_c,
00085 const p_array<P>& ck,
00086 const M& map,
00087 const algebra::vec<3u,float>& mu_xk)
00088 {
00089
00090 mln_precondition(3u == 3);
00091
00092
00093 algebra::mat<3u,3u,float> Mk(literal::zero);
00094 for (unsigned i = 0; i < c.nsites(); ++i)
00095 {
00096 algebra::vec<3u,float> ci = convert::to< algebra::vec<3u,float> >(c[i]);
00097 algebra::vec<3u,float> xki = convert::to< algebra::vec<3u,float> >(map(ck[i]));
00098 Mk += (ci - mu_c) * (xki - mu_xk).t();
00099 }
00100 Mk /= c.nsites();
00101
00102 algebra::vec<3u,float> a;
00103 a[0] = Mk(1,2) - Mk(2,1);
00104 a[1] = Mk(2,0) - Mk(0,2);
00105 a[2] = Mk(0,1) - Mk(1,0);
00106
00107 algebra::mat<4u,4u,float> Qk(literal::zero);
00108 float t = tr(Mk);
00109
00110 Qk(0,0) = t;
00111 for (int i = 1; i < 4; i++)
00112 {
00113 Qk(i,0) = a[i - 1];
00114 Qk(0,i) = a[i - 1];
00115 for (int j = 1; j < 4; j++)
00116 if (i == j)
00117 Qk(i,j) = 2 * Mk(i - 1,i - 1) - t;
00118 }
00119
00120 Qk(1,2) = Mk(0,1) + Mk(1,0);
00121 Qk(2,1) = Mk(0,1) + Mk(1,0);
00122
00123 Qk(1,3) = Mk(0,2) + Mk(2,0);
00124 Qk(3,1) = Mk(0,2) + Mk(2,0);
00125
00126 Qk(2,3) = Mk(1,2) + Mk(2,1);
00127 Qk(3,2) = Mk(1,2) + Mk(2,1);
00128
00129 algebra::quat qR(literal::zero);
00130 qR = math::jacobi(Qk);
00131
00132 std::cout << qR << std::endl;
00133
00134 return fun::x2x::rotation<3u, float>(qR);
00135 }
00136
00137 # endif // ! MLN_INCLUDE_ONLY
00138
00139
00140 }
00141
00142 }
00143
00144 #endif // ! MLN_REGISTRATION_GET_ROT_HH