[Olena-patches] 2612: Registration Fix.

https://svn.lrde.epita.fr/svn/oln/branches/cleanup-2008/milena Index: ChangeLog from Ugo Jardonnet &lt;jardonnet(a)lrde.epita.fr&gt; Registration Fix. * mln/convert/to_image.hh: Fix missing ;. * mln/convert/to_p_array.hh: Fix use of mln_point. * mln/fun/x2v/all.hh: Fix wrong comment. * mln/fun/x2x/rotation.hh: Add constructor with quaternion. * mln/fun/x2p/closest_point.hh: Fix warning. * mln/algebra/quat.hh (quad::rotate): Add method. * mln/registration/get_rot.hh: New. Get rotation needed btw 2 p_arrays. * mln/registration/registration.hh: Add inclusion. * mln/registration/icp.hh: Minor fix. * mln/registration/get_rtransf.hh: Split code. * mln/registration/internal/rms.hh: Add specific rms computation. algebra/quat.hh | 11 +++ convert/to_image.hh | 4 - convert/to_p_array.hh | 4 - fun/x2p/closest_point.hh | 2 fun/x2v/all.hh | 2 fun/x2x/rotation.hh | 32 +++++++++- registration/get_rot.hh | 128 +++++++++++++++++++++++++++++++++++++++++++ registration/get_rtransf.hh | 46 +-------------- registration/icp.hh | 7 +- registration/internal/rms.hh | 78 ++++++++++++++++++++++++++ registration/registration.hh | 6 +- 11 files changed, 262 insertions(+), 58 deletions(-) Index: mln/convert/to_image.hh --- mln/convert/to_image.hh (revision 2611) +++ mln/convert/to_image.hh (working copy) @@ -124,9 +124,9 @@ template <typename W> mln_image_from(W, mln_weight(W)) to_image(const Weighted_Window<W>& w_win); - /// Convert an histo \p h into an image1d<std::size_t>. + /// Convert an histo \p h into an image1d<T>. template <typename T> - image1d<T> to_image(const histo::data<T>& h) + image1d<T> to_image(const histo::data<T>& h); Index: mln/convert/to_p_array.hh --- mln/convert/to_p_array.hh (revision 2611) +++ mln/convert/to_p_array.hh (working copy) @@ -92,12 +92,12 @@ template <typename I> inline - p_array<mln_point(I)> + p_array<mln_psite(I)> to_p_array(const Image<I>& img_) { const I& img = exact(img_); - p_array<mln_point(I)> a; + p_array<mln_psite(I)> a; mln_piter(I) p(img.domain()); for_all(p) Index: mln/fun/x2v/all.hh --- mln/fun/x2v/all.hh (revision 2611) +++ mln/fun/x2v/all.hh (working copy) @@ -44,7 +44,7 @@ namespace x2v { - /// Internal namespace of functions form vector to vector. + /// Internal namespace of functions form vector to value. namespace internal { } Index: mln/fun/x2x/rotation.hh --- mln/fun/x2x/rotation.hh (revision 2611) +++ mln/fun/x2x/rotation.hh (working copy) @@ -37,6 +37,7 @@ # include <mln/fun/internal/x2x_linear_impl.hh> # include <mln/algebra/vec.hh> # include <mln/algebra/mat.hh> +# include <mln/algebra/quat.hh> # include <cmath> namespace mln @@ -106,14 +107,13 @@ const float sin_a = sin(alpha_); m_(0,0) = cos_a; m_(0,1) = -sin_a; m_(0,2) = 0; - m_(1,0) = sin_a; m_(1,1) = cos_a; m_(1,2) = 0; - m_(2,0) = 0; m_(2,1) = 0; m_(2,2) = 1; return m_; } - } + + } // end of namespace internal /*! \brief Represent a rotation function. @@ -135,6 +135,8 @@ rotation(); /// Constructor with grade alpha and a facultative direction (rotation axis). rotation(float alpha, const algebra::vec<n,float>& axis); + /// Constructor with quaternion + rotation(const algebra::quat& q); using super_::operator(); /// Perform the rotation of the given vector. @@ -143,7 +145,7 @@ /// Set a new grade alpha. void set_alpha(float alpha); /// Set a new rotation axis. - void set_dir(unsigned dir); + void set_axis(const algebra::vec<n,float>& axis); protected: void update(); @@ -173,6 +175,25 @@ template <unsigned n, typename C> inline + rotation<n,C>::rotation(const algebra::quat& q) + { + mln_precondition(q.is_unit()); + // FIXME: Should also work for 2d. + mln_precondition(n == 3); + float + w = q.to_vec()[0], + x = q.to_vec()[1], x2 = 2*x*x, xw = 2*x*w, + y = q.to_vec()[2], y2 = 2*y*y, xy = 2*x*y, yw = 2*y*w, + z = q.to_vec()[3], z2 = 2*z*z, xz = 2*x*z, yz = 2*y*z, zw = 2*z*w; + float t[16] = {1.f - y2 - z2, xy - zw, xz + yw, 0, + xy + zw, 1.f - x2 - z2, yz - xw, 0, + xz - yw, yz + xw, 1.f - x2 - y2, 0, + 0, 0, 0, 1}; + this->m_(make::mat<4,4,16,float>(t)); + } + + template <unsigned n, typename C> + inline algebra::vec<n,C> rotation<n,C>::operator()(const algebra::vec<n,C>& v) const { @@ -211,8 +232,9 @@ template <unsigned n, typename C> inline void - rotation<n,C>::set_dir(unsigned dir) + rotation<n,C>::set_axis(const algebra::vec<n,float>& axis) { + axis_ = axis; update(); } Index: mln/fun/x2p/closest_point.hh --- mln/fun/x2p/closest_point.hh (revision 2611) +++ mln/fun/x2p/closest_point.hh (working copy) @@ -49,7 +49,7 @@ typedef P result; closest_point(const p_array<P>& X, const box<P>& box) - : X(X), box_(box) + : box_(box), X(X) , log_functor_call(0) { } Index: mln/algebra/quat.hh --- mln/algebra/quat.hh (revision 2611) +++ mln/algebra/quat.hh (working copy) @@ -192,6 +192,10 @@ /// Transform into unit quaternion. quat& set_unit(); + /// Rotate using quaternion definition of a rotation + template <unsigned n> + algebra::vec<n,float> rotate(const algebra::vec<n,float>& v); + /// Transform into unit quaternion. template <typename T> void set_unit(float theta, const algebra::vec<3,T>& uv); @@ -475,6 +479,13 @@ set_unit(theta(), uv); } + template <unsigned n> + inline + algebra::vec<n,float> + quat::rotate(const algebra::vec<n,float>& v) + { + return ((*this) * quat(0. ,v) * (*this).inv()).v(); + } // Operators. Index: mln/registration/get_rot.hh --- mln/registration/get_rot.hh (revision 0) +++ mln/registration/get_rot.hh (revision 0) @@ -0,0 +1,128 @@ +// Copyright (C) 2008 EPITA Research and Development Laboratory +// +// This file is part of the Olena Library. This library is free +// software; you can redistribute it and/or modify it under the terms +// of the GNU General Public License version 2 as published by the +// Free Software Foundation. +// +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this library; see the file COPYING. If not, write to +// the Free Software Foundation, 51 Franklin Street, Fifth Floor, +// Boston, MA 02111-1307, USA. +// +// As a special exception, you may use this file as part of a free +// software library without restriction. Specifically, if other files +// instantiate templates or use macros or inline functions from this +// file, or you compile this file and link it with other files to +// produce an executable, this file does not by itself cause the +// resulting executable to be covered by the GNU General Public +// License. This exception does not however invalidate any other +// reasons why the executable file might be covered by the GNU General +// Public License. + +#ifndef MLN_REGISTRATION_GET_ROT_HH +# define MLN_REGISTRATION_GET_ROT_HH + +# include <mln/fun/x2x/all.hh> +# include <mln/algebra/quat.hh> +# include <mln/math/jacobi.hh> + +namespace mln +{ + + namespace registration + { + + template <typename P, typename M> + fun::x2x::rotation<P::dim, float> + get_rot(const p_array<P>& c, + const algebra::vec<P::dim,float>& mu_c, + const p_array<P>& ck, + const M& map, + const algebra::vec<P::dim,float>& mu_xk); + + +# ifndef MLN_INCLUDE_ONLY + + + template <typename P, typename M> + fun::x2x::rotation<2u, float> + get_rot(const p_array<P>& c, + const algebra::vec<2u,float>& mu_c, + const p_array<P>& ck, + const M& map, + const algebra::vec<2u,float>& mu_xk) + { + //FIXME: Write 2d version of rotation computation betwenn two p_array + return fun::x2x::rotation<2u, float>(); + } + + template <typename P, typename M> + fun::x2x::rotation<3u, float> + get_rot(const p_array<P>& c, + const algebra::vec<3u,float>& mu_c, + const p_array<P>& ck, + const M& map, + const algebra::vec<3u,float>& mu_xk) + { + //FIXME: Make assertion static + mln_precondition(3u == 3); + + // FIXME: Make use of a cross_covariance accu (maybe not because of map(ck[i])) + algebra::mat<3u,3u,float> Mk(literal::zero); + for (unsigned i = 0; i < c.nsites(); ++i) + { + algebra::vec<3u,float> ci = c[i]; + algebra::vec<3u,float> xki = map(ck[i]); + Mk += make::mat(ci - mu_c) * trans(make::mat(xki - mu_xk)); + } + Mk /= c.nsites(); + + algebra::vec<3u,float> a; + a[0] = Mk(1,2) - Mk(2,1); + a[1] = Mk(2,0) - Mk(0,2); + a[2] = Mk(0,1) - Mk(1,0); + + algebra::mat<4u,4u,float> Qk(literal::zero); + float t = tr(Mk); + + Qk(0,0) = t; + for (int i = 1; i < 4; i++) + { + Qk(i,0) = a[i - 1]; + Qk(0,i) = a[i - 1]; + for (int j = 1; j < 4; j++) + if (i == j) + Qk(i,j) = 2 * Mk(i - 1,i - 1) - t; + } + + Qk(1,2) = Mk(0,1) + Mk(1,0); + Qk(2,1) = Mk(0,1) + Mk(1,0); + + Qk(1,3) = Mk(0,2) + Mk(2,0); + Qk(3,1) = Mk(0,2) + Mk(2,0); + + Qk(2,3) = Mk(1,2) + Mk(2,1); + Qk(3,2) = Mk(1,2) + Mk(2,1); + + algebra::quat qR(literal::zero); + qR = math::jacobi(Qk); + + return fun::x2x::rotation<3u, float>(qR); + } + +# endif // ! MLN_INCLUDE_ONLY + + + } // end of namespace registration + +} // end of namespace mln + +#endif /* MLN_REGISTRATION_GET_ROT_HH */ + + Index: mln/registration/registration.hh --- mln/registration/registration.hh (revision 2611) +++ mln/registration/registration.hh (working copy) @@ -37,6 +37,7 @@ # include <mln/fun/x2x/all.hh> # include <mln/fun/x2p/closest_point.hh> # include <mln/core/image/lazy_image.hh> +# include <mln/convert/to_p_array.hh> namespace mln { @@ -66,8 +67,9 @@ registration_(const I& cloud, const J& surface) { - p_array<mln_psite(I)> c = convert::to< p_array<mln_psite(I)> - p_array<mln_psite(J)> x = convert::to< p_array<mln_psite(I)> + //FIXME: Use convert::to< p_array<mln_psite(I)> >() + p_array<mln_psite(I)> c = convert::to_p_array(cloud); + p_array<mln_psite(J)> x = convert::to_p_array(surface); //init rigid transform qk composed< rotation<I::psite::dim, float>, translation<I::psite::dim, float> > qk; Index: mln/registration/icp.hh --- mln/registration/icp.hh (revision 2611) +++ mln/registration/icp.hh (working copy) @@ -36,6 +36,7 @@ # include <mln/fun/x2x/all.hh> # include <mln/fun/x2v/all.hh> # include <mln/registration/get_rtransf.hh> +# include <mln/registration/internal/rms.hh> namespace mln { @@ -180,10 +181,12 @@ // d_k = d(Yk, Pk+1) // = d(closest(qk-1(P)), qk(P)) - d_k = rms(c, map, buf_qk[1], qk); + d_k = internal::rms(c, map, buf_qk[1], qk); k++; - } while ((qk - buf_qk[1]).sqr_norm() / qk.sqr_norm() > epsilon); + //FIXME: Add matrix norm + //} while ((qk - buf_qk[1]).sqr_norm() / qk.sqr_norm() > epsilon); + } while (d_k > epsilon); trace::exiting("registration::impl::icp_"); } Index: mln/registration/get_rtransf.hh --- mln/registration/get_rtransf.hh (revision 2611) +++ mln/registration/get_rtransf.hh (working copy) @@ -31,6 +31,7 @@ # include <mln/fun/x2x/all.hh> # include <mln/algebra/quat.hh> # include <mln/math/jacobi.hh> +# include <mln/registration/get_rot.hh> namespace mln { @@ -50,7 +51,6 @@ # ifndef MLN_INCLUDE_ONLY - template <typename P, typename M> composed<rotation<P::dim, float>, translation<P::dim, float> > get_rtransf(const p_array<P>& c, @@ -64,51 +64,11 @@ mu_xk += convert::to< algebra::vec<P::dim,float> >(map(ck[i])); mu_xk /= c.nsites(); - // FIXME: Make use of a cross_covariance accu (maybe not because of map(ck[i])) // qR - algebra::mat<P::dim,P::dim,float> Mk(literal::zero); - for (unsigned i = 0; i < c.nsites(); ++i) - { - algebra::vec<P::dim,float> ci = c[i]; - algebra::vec<P::dim,float> xki = map(ck[i]); - Mk += make::mat(ci - mu_c) * trans(make::mat(xki - mu_xk)); - } - Mk /= c.nsites(); - - algebra::vec<3,float> a; - a[0] = Mk(1,2) - Mk(2,1); - a[1] = Mk(2,0) - Mk(0,2); - a[2] = Mk(0,1) - Mk(1,0); - - algebra::mat<4,4,float> Qk(literal::zero); - float t = tr(Mk); - - Qk(0,0) = t; - for (int i = 1; i < 4; i++) - { - Qk(i,0) = a[i - 1]; - Qk(0,i) = a[i - 1]; - for (int j = 1; j < 4; j++) - if (i == j) - Qk(i,j) = 2 * Mk(i - 1,i - 1) - t; - } - - Qk(1,2) = Mk(0,1) + Mk(1,0); - Qk(2,1) = Mk(0,1) + Mk(1,0); - - Qk(1,3) = Mk(0,2) + Mk(2,0); - Qk(3,1) = Mk(0,2) + Mk(2,0); - - Qk(2,3) = Mk(1,2) + Mk(2,1); - Qk(3,2) = Mk(1,2) + Mk(2,1); - - algebra::quat qR(literal::zero); - qR = math::jacobi(Qk); + rotation<P::dim, float> tqR = get_rot(c, mu_c, ck, map, mu_xk); // qT - const algebra::vec<P::dim,float> qT = mu_xk - rotate(qR, mu_c); - - rotation<P::dim, float> tqR(qR); + const algebra::vec<P::dim,float> qT = mu_xk - tqR(mu_c); translation<P::dim, float> tqT(qT); return compose(tqR,tqT); } Index: mln/registration/internal/rms.hh --- mln/registration/internal/rms.hh (revision 0) +++ mln/registration/internal/rms.hh (revision 0) @@ -0,0 +1,78 @@ +// Copyright (C) 2008 EPITA Research and Development Laboratory +// +// This file is part of the Olena Library. This library is free +// software; you can redistribute it and/or modify it under the terms +// of the GNU General Public License version 2 as published by the +// Free Software Foundation. +// +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this library; see the file COPYING. If not, write to +// the Free Software Foundation, 51 Franklin Street, Fifth Floor, +// Boston, MA 02111-1307, USA. +// +// As a special exception, you may use this file as part of a free +// software library without restriction. Specifically, if other files +// instantiate templates or use macros or inline functions from this +// file, or you compile this file and link it with other files to +// produce an executable, this file does not by itself cause the +// resulting executable to be covered by the GNU General Public +// License. This exception does not however invalidate any other +// reasons why the executable file might be covered by the GNU General +// Public License. + +#ifndef MLN_REGISTRATION_INTERNAL_RMS_HH +# define MLN_REGISTRATION_INTERNAL_RMS_HH + +# include <mln/core/site_set/p_array.hh> + +namespace mln +{ + + namespace registration + { + + namespace internal + { + + template <typename P, typename M, typename T> + float rms(const p_array<P>& a1, + M& map, + const unsigned length, + const T& q1, + const T& q2); + +# ifndef MLN_INCLUDE_ONLY + + + template <typename P, typename M, typename T> + float rms(const p_array<P>& a, + M& map, + const T& q1, + const T& q2) + { + float f = 0.f; + for (unsigned i = 0; i < a.nsites(); ++i) + { + algebra::vec<P::dim,float> a2f = q2(a[i]); + algebra::vec<P::dim,float> a1f = map(algebra::to_point<P>(q1(a[i]))); + f += norm::l2(a1f - a2f); + } + return f / a.nsites(); + } + + +# endif // ! MLN_INCLUDE_ONLY + + } // end of nqmespace mln::registration::internal + + } // end of namespace mln::registration + +} // end of namespace mln + + +#endif // ! MLN_REGISTRATION_INTERNAL_RMS_HH