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_RTRANSF_HH
00027 # define MLN_REGISTRATION_GET_RTRANSF_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 # include <mln/registration/get_rot.hh>
00035
00036 namespace mln
00037 {
00038
00039 namespace registration
00040 {
00041
00042 using namespace fun::x2x;
00043
00044 template <typename P, typename M>
00045 composed<rotation<P::dim, float>, translation<P::dim, float> >
00046 get_rtransf(const p_array<P>& C,
00047 const algebra::vec<P::dim,float>& mu_C,
00048 const p_array<P>& ck,
00049 const M& map);
00050
00051
00052 # ifndef MLN_INCLUDE_ONLY
00053
00054 template <typename P, typename M>
00055 composed<rotation<P::dim, float>, translation<P::dim, float> >
00056 get_rtransf(const p_array<P>& c,
00057 const algebra::vec<P::dim,float>& mu_c,
00058 const p_array<P>& ck,
00059 const M& map)
00060 {
00061
00062 algebra::vec<P::dim,float> mu_xk(literal::zero);
00063
00064 std::cout << c.nsites() << std::endl;
00065 for (unsigned i = 0; i < c.nsites(); ++i)
00066 {
00067 if (not map.has(ck[i]))
00068 {
00069 std::cout << ck[i] << " " << i << std::endl;
00070 }
00071 else
00072 mu_xk += convert::to< algebra::vec<P::dim,float> >
00073 (map(convert::to< algebra::vec<P::dim,float> >(ck[i])));
00074
00075 }
00076 mu_xk /= c.nsites();
00077
00078
00079 rotation<P::dim, float> tqR = get_rot(c, mu_c, ck, map, mu_xk);
00080
00081
00082 const algebra::vec<P::dim,float> qT = mu_xk - tqR(mu_c);
00083 translation<P::dim, float> tqT(qT);
00084 return compose(tqR,tqT);
00085 }
00086
00087 # endif // ! MLN_INCLUDE_ONLY
00088
00089
00090 }
00091
00092 }
00093
00094 #endif // ! MLN_REGISTRATION_GET_RTRANSF_HH