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