https://svn.lrde.epita.fr/svn/oln/branches/cleanup-2008/milena Index: ChangeLog from Ugo Jardonnet <ugo.jardonnet@lrde.epita.fr> Update registration. * mln/core/site_set/box.hh (larger_than): Add. * mln/math/jacobi.hh: Add jacobi eigen vector extraction method. * mln/registration/registration.hh: Minor fix. * mln/registration/icp.hh: Fix. * mln/registration/get_rtransf.hh: Add match function. core/site_set/box.hh | 22 ++++++ math/jacobi.hh | 155 +++++++++++++++++++++++++++++++++++++++++++ registration/get_rtransf.hh | 124 ++++++++++++++++++++++++++++++++++ registration/icp.hh | 54 +++++++++++--- registration/registration.hh | 8 +- 5 files changed, 345 insertions(+), 18 deletions(-) Index: mln/core/site_set/box.hh --- mln/core/site_set/box.hh (revision 2544) +++ mln/core/site_set/box.hh (working copy) @@ -165,6 +165,13 @@ template <typename P> std::ostream& operator<<(std::ostream& ostr, const box<P>& b); + // Procedures + + /// Return the minimum box including box \p a and box \b + template <typename P> + inline + box<P> + larger_than(const box<P> a, const box<P> b); # ifndef MLN_INCLUDE_ONLY @@ -287,6 +294,21 @@ mln_postcondition(is_valid()); } + template <typename P> + inline + box<P> + larger_than(const box<P> a, const box<P> b) + { + P pmin,pmax; + + for (unsigned i = 0; i < P::dim; i++) + { + pmin[i] = (a.pmin()[i] < b.pmin()[i]) ? a.pmin()[i] : b.pmin()[i]; + pmax[i] = (a.pmax()[i] > b.pmax()[i]) ? a.pmax()[i] : b.pmax()[i]; + } + + return box<P>(pmin, pmax); + } template <typename P> inline Index: mln/math/jacobi.hh --- mln/math/jacobi.hh (revision 0) +++ mln/math/jacobi.hh (revision 0) @@ -0,0 +1,155 @@ +// 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_MATH_JACOBI_HH +# define MLN_MATH_JACOBI_HH + +# include <mln/algebra/quat.hh> +# include <mln/algebra/mat.hh> + +// from num. rec. in C +// FIXME: what about numrec licence? + +namespace mln +{ + + namespace math + { + + algebra::quat jacobi(algebra::mat < 4, 4, float >a); + +# ifndef MLN_INCLUDE_ONLY + + // FIXME: nD ? +#define rotateJacobi(a,i,j,k,l) g=a(i,j);h=a(k,l);a(i,j)=g-s*(h+g*tau); \ + a(k,l)=h+s*(g-h*tau) + + algebra::quat jacobi(algebra::mat < 4, 4, float >a) + { + float dd, d[4]; + algebra::mat < 4, 4, float >v(literal::zero); + int j, iq, ip, i = 0; + float tresh, theta, tau, t, sm, s, h, g, c, b[4], z[4]; + for (ip = 0; ip < 4; ip++) + { + for (iq = 0; iq < 4; iq++) + v(ip, iq) = 0.0; + v(ip, ip) = 1.0; + } + for (ip = 0; ip < 4; ip++) + { + b[ip] = d[ip] = a(ip, ip); + z[ip] = 0.0; + } + while (1) + { + sm = 0.0; + for (ip = 0; ip < 3; ip++) + { + for (iq = ip + 1; iq < 4; iq++) + sm += fabs(a(ip, iq)); + } + if (sm < 1e-12) + { // 1e-12 + dd = d[0]; + iq = 0; + for (ip = 1; ip < 4; ip++) + if (d[ip] > dd) + { + iq = ip; + dd = d[ip]; + } + algebra::quat q(v(0, iq), v(1, iq), v(2, iq), v(3, iq)); + q.set_unit(); + return q; + } + if (i < 4) + { + i++; + tresh = 0.0125 * sm; + } + else + tresh = 0.0; + for (ip = 0; ip < 3; ip++) + { + for (iq = ip + 1; iq < 4; iq++) + { + g = 100.0 * fabs(a(ip, iq)); + if (i > 4 && (float)(fabs(d[ip])+g) == + (float)fabs(d[ip]) + && (float)(fabs(d[iq])+g) == (float)fabs(d[iq])) + a(ip, iq) = 0.0; + else if (fabs(a(ip, iq)) > tresh) + { + h = d[iq] - d[ip]; + if ((float)(fabs(h)+g) == (float)fabs(h)) // unsafe? + t = (a(ip, iq)) / h; + else + { + theta = 0.5 * h / (a(ip, iq)); + t = 1.0 / (fabs(theta) + sqrt(1.0 + + theta * theta)); + if (theta < 0.0) + t = -t; + } + c = 1.0 / sqrt(1 + t * t); + s = t * c; + tau = s / (1.0 + c); + h = t * a(ip, iq); + z[ip] -= h; + z[iq] += h; + d[ip] -= h; + d[iq] += h; + a(ip, iq) = 0.0; + for (j = 0; j <= ip - 1; j++) + rotateJacobi(a, j, ip, j, iq); + for (j = ip + 1; j <= iq - 1; j++) + rotateJacobi(a, ip, j, j, iq); + for (j = iq + 1; j < 4; j++) + rotateJacobi(a, ip, j, iq, j); + for (j = 0; j < 4; j++) + rotateJacobi(v, j, ip, j, iq); + } + } + } + for (ip = 0; ip < 4; ip++) + { + b[ip] += z[ip]; + d[ip] = b[ip]; + z[ip] = 0.0; + } + } + } + +# endif // ! MLN_INCLUDE_ONLY + + } // end of namespace math + +} // end of namespace mln + +#endif /* MLN_MATH_JACOBI_HH */ + Index: mln/registration/registration.hh --- mln/registration/registration.hh (revision 2544) +++ mln/registration/registration.hh (working copy) @@ -34,6 +34,7 @@ */ # include <mln/registration/icp.hh> +# include <mln/fun/x2x/all.hh> # include <mln/fun/x2p/closest_point.hh> # include <mln/core/image/lazy_image.hh> @@ -43,8 +44,7 @@ namespace registration { - - using namespace fun::x2x; + using namespace mln::fun::x2x; /*! Register an image \p cloud over the image \p surface. */ @@ -74,7 +74,7 @@ //working box const box<mln_psite(I)> working_box = - enlarge(bigger(geom::bbox(c), geom::bbox(x)), 100); + larger_than(geom::bbox(c), geom::bbox(x)).to_larger(100); //make a lazy_image map via function closest_point fun::x2p::closest_point<mln_psite(I)> fun(x, working_box); @@ -83,7 +83,7 @@ //run registration - registration::icp(c, map, qk, c.nsites(), 1e-3); + return registration::icp(c, map, 1e-3); } Index: mln/registration/icp.hh --- mln/registration/icp.hh (revision 2544) +++ mln/registration/icp.hh (working copy) @@ -35,6 +35,7 @@ # include <mln/fun/x2x/all.hh> # include <mln/fun/x2v/all.hh> +# include <mln/registration/get_rtransf.hh> namespace mln { @@ -95,7 +96,7 @@ */ template <typename P, typename M> inline - composed<rotation<2u, float>, translation<2u, float> > + composed<rotation<P::dim, float>, translation<P::dim, float> > icp(const p_array<P>& c, const M& map, const float epsilon = 1e-3); @@ -117,10 +118,32 @@ namespace impl { + // FIXME: Move elsewhere + template <typename P> + P center(const p_array<P>& a) + { + algebra::vec<P::dim,float> c(literal::zero); + for (unsigned i = 0; i < a.nsites(); ++i) + c += convert::to< algebra::vec<P::dim,float> > (a[i]); + + return algebra::to_point<P>(c / a.nsites()); + } + + // FIXME: Make use of level::apply + template <typename P, typename F> + void apply(F& f, + const p_array<P>& a, + p_array<P>& b) + { + for (unsigned i = 0; i < a.nsites(); i++) + b[i] = f(a[i]); + } + + template <typename P, typename M, typename T> inline void - icp_(const p_array<P>& c, + icp_(const p_array<P>& c_, const unsigned c_length, const M& map, T& qk, @@ -132,28 +155,32 @@ util::buffer<3,float> buf_dk; float d_k = 10000; - p_array<P> Ck(c); - algebra::vec<P::dim,float> mu_C = center(c, c_length), mu_Xk; + //work on a reduced version of c_ + p_array<P> c = c_; + c.hook_std_vector_().resize(c_length); + + p_array<P> ck(c); + algebra::vec<P::dim,float> mu_c = center(c); buf_qk.store(qk); - qk.apply_on(c, Ck, c_length); + apply(qk, c, ck); unsigned int k = 0; do { buf_dk.store(d_k); //compute qk - qk = match(c, mu_C, Ck, map, c_length); + qk = get_rtransf(c, mu_c, ck, map); buf_qk.store(qk); //Ck+1 = qk(c) - qk.apply_on(c, Ck, c_length); + apply(qk, c, ck); // d_k = d(Yk, Pk+1) // = d(closest(qk-1(P)), qk(P)) - d_k = rms(c, map, c_length, buf_qk[1], qk); + d_k = rms(c, map, buf_qk[1], qk); k++; } while ((qk - buf_qk[1]).sqr_norm() / qk.sqr_norm() > epsilon); @@ -164,15 +191,14 @@ } // end of namespace mln::registration::impl - template <typename P, typename M, typename T> + template <typename P, typename M> inline - T - icp(const p_array<P>& c, - const M& map, + composed<rotation<P::dim, float>, translation<P::dim, float> > + icp(const p_array<P>& c, const M& map, const float epsilon = 1e-3) { - T qk; - impl::icp_(c, map, qk, c.length(), epsilon); + composed<rotation<P::dim, float>, translation<P::dim, float> > qk; + impl::icp_(c, c.nsites(),map, qk, epsilon); return qk; } Index: mln/registration/get_rtransf.hh --- mln/registration/get_rtransf.hh (revision 0) +++ mln/registration/get_rtransf.hh (revision 0) @@ -0,0 +1,124 @@ +// 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_RTRANSF_HH +# define MLN_REGISTRATION_GET_RTRANSF_HH + +# include <mln/fun/x2x/all.hh> +# include <mln/algebra/quat.hh> +# include <mln/math/jacobi.hh> + +namespace mln +{ + + namespace registration + { + + using namespace fun::x2x; + + template <typename P, typename M> + composed<rotation<P::dim, float>, translation<P::dim, float> > + get_rtransf(const p_array<P>& C, + const algebra::vec<P::dim,float>& mu_C, + const p_array<P>& ck, + const M& map); + + +# 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, + const algebra::vec<P::dim,float>& mu_c, + const p_array<P>& ck, + const M& map) + { + //mu_xk = center map(Ck) + algebra::vec<P::dim,float> mu_xk(literal::zero); + for (unsigned i = 0; i < c.nsites(); ++i) + 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); + + // qT + const algebra::vec<P::dim,float> qT = mu_xk - rotate(qR, mu_c); + + rotation<P::dim, float> tqR(qR); + translation<P::dim, float> tqT(qT); + return compose(tqR,tqT); + } + +# endif // ! MLN_INCLUDE_ONLY + + + } // end of namespace registration + +} // end of namespace mln + +#endif /* MLN_REGISTRATION_GET_RTRANSF_HH */ +