Milena (Olena)  User documentation 2.0a Id
 All Classes Namespaces Functions Variables Typedefs Enumerator Groups Pages
get_rot.hh
1 // Copyright (C) 2008, 2009, 2011 EPITA Research and Development
2 // Laboratory (LRDE)
3 //
4 // This file is part of Olena.
5 //
6 // Olena is free software: you can redistribute it and/or modify it under
7 // the terms of the GNU General Public License as published by the Free
8 // Software Foundation, version 2 of the License.
9 //
10 // Olena is distributed in the hope that it will be useful,
11 // but WITHOUT ANY WARRANTY; without even the implied warranty of
12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 // General Public License for more details.
14 //
15 // You should have received a copy of the GNU General Public License
16 // along with Olena. If not, see <http://www.gnu.org/licenses/>.
17 //
18 // As a special exception, you may use this file as part of a free
19 // software project without restriction. Specifically, if other files
20 // instantiate templates or use macros or inline functions from this
21 // file, or you compile this file and link it with other files to produce
22 // an executable, this file does not by itself cause the resulting
23 // executable to be covered by the GNU General Public License. This
24 // exception does not however invalidate any other reasons why the
25 // executable file might be covered by the GNU General Public License.
26 
27 #ifndef MLN_REGISTRATION_GET_ROT_HH
28 # define MLN_REGISTRATION_GET_ROT_HH
29 
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>
35 
36 
37 namespace mln
38 {
39 
40  namespace registration
41  {
42 
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,
47  const p_array<P>& ck,
48  const M& map,
49  const algebra::vec<P::dim,float>& mu_xk);
50 
51 
52 # ifndef MLN_INCLUDE_ONLY
53 
54 
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,
59  const p_array<P>& ck,
60  const M& map,
61  const algebra::vec<2u,float>& mu_xk)
62  {
63  assert(0 && "TODO");
64 
65  (void) c;
66  (void) mu_c;
67  (void) ck;
68  (void) map;
69  (void) mu_xk;
70 
72 
74  // M1 := c covariance
75  // V1 := greatest eigen vector of M1
76 
78  // M2 := c covariance
79  // V2 := greatest eigen vector of M2
80 
82  // cos(alpha) = (V1.V2) / (|V1|.|V2|)
83 
84  //FIXME: Write 2d version of rotation computation between two p_arrays
85  return fun::x2x::rotation<2u, float>();
86  }
87 
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,
92  const p_array<P>& ck,
93  const M& map,
94  const algebra::vec<3u,float>& mu_xk)
95  {
96  //FIXME: Make assertion static
97  mln_precondition(3u == 3);
98 
99  // FIXME: Make use of a cross_covariance accu (maybe not because of map(ck[i]))
100  algebra::mat<3u,3u,float> Mk(literal::zero);
101  for (unsigned i = 0; i < c.nsites(); ++i)
102  {
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();
106  }
107  Mk /= c.nsites();
108 
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);
113 
114  algebra::mat<4u,4u,float> Qk(literal::zero);
115  float t = tr(Mk);
116 
117  Qk(0,0) = t;
118  for (int i = 1; i < 4; i++)
119  {
120  Qk(i,0) = a[i - 1];
121  Qk(0,i) = a[i - 1];
122  for (int j = 1; j < 4; j++)
123  if (i == j)
124  Qk(i,j) = 2 * Mk(i - 1,i - 1) - t;
125  }
126 
127  Qk(1,2) = Mk(0,1) + Mk(1,0);
128  Qk(2,1) = Mk(0,1) + Mk(1,0);
129 
130  Qk(1,3) = Mk(0,2) + Mk(2,0);
131  Qk(3,1) = Mk(0,2) + Mk(2,0);
132 
133  Qk(2,3) = Mk(1,2) + Mk(2,1);
134  Qk(3,2) = Mk(1,2) + Mk(2,1);
135 
136  algebra::quat qR(literal::zero);
137  qR = math::jacobi(Qk);
138 
139  std::cout << qR << std::endl;
140 
141  return fun::x2x::rotation<3u, float>(qR);
142  }
143 
144 # endif // ! MLN_INCLUDE_ONLY
145 
146 
147  } // end of namespace registration
148 
149 } // end of namespace mln
150 
151 #endif // ! MLN_REGISTRATION_GET_ROT_HH