• Main Page
  • Related Pages
  • Modules
  • Namespaces
  • Classes
  • Files
  • File List

get_rot.hh

00001 // Copyright (C) 2008, 2009, 2011 EPITA Research and Development
00002 // Laboratory (LRDE)
00003 //
00004 // This file is part of Olena.
00005 //
00006 // Olena is free software: you can redistribute it and/or modify it under
00007 // the terms of the GNU General Public License as published by the Free
00008 // Software Foundation, version 2 of the License.
00009 //
00010 // Olena is distributed in the hope that it will be useful,
00011 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00012 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00013 // General Public License for more details.
00014 //
00015 // You should have received a copy of the GNU General Public License
00016 // along with Olena.  If not, see <http://www.gnu.org/licenses/>.
00017 //
00018 // As a special exception, you may use this file as part of a free
00019 // software project without restriction.  Specifically, if other files
00020 // instantiate templates or use macros or inline functions from this
00021 // file, or you compile this file and link it with other files to produce
00022 // an executable, this file does not by itself cause the resulting
00023 // executable to be covered by the GNU General Public License.  This
00024 // exception does not however invalidate any other reasons why the
00025 // executable file might be covered by the GNU General Public License.
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       // M1 := c covariance
00075       // V1 := greatest eigen vector of M1
00076 
00078       // M2 := c covariance
00079       // V2 := greatest eigen vector of M2
00080 
00082       // cos(alpha) = (V1.V2) / (|V1|.|V2|)
00083 
00084       //FIXME: Write 2d version of rotation computation between two p_arrays
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       //FIXME: Make assertion static
00097       mln_precondition(3u == 3);
00098 
00099       // FIXME: Make use of a cross_covariance accu (maybe not because of map(ck[i]))
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   } // end of namespace registration
00148 
00149 } // end of namespace mln
00150 
00151 #endif // ! MLN_REGISTRATION_GET_ROT_HH

Generated on Mon Jan 30 2012 17:35:53 for Milena (Olena) by  doxygen 1.7.1