[Olena-patches] cleanup-2008 2433: Sandbox: Start revamp registration.

https://svn.lrde.epita.fr/svn/oln/branches/cleanup-2008/milena/sandbox Index: ChangeLog from Ugo Jardonnet &lt;ugo.jardonnet(a)lrde.epita.fr&gt; Sandbox: Start revamp registration. * jardonnet/test/registration.cc: Simple registration test. * jardonnet/registration/registration.hh: Simple registration. * jardonnet/registration/multiscale.hh: Multiscale registration. * jardonnet/test/icp.cc, * jardonnet/registration/icp.hh, * jardonnet/registration/final_qk.hh, * jardonnet/registration/save.hh, * jardonnet/registration/tools.hh, * jardonnet/registration/icp_ref.hh, * jardonnet/registration/quat7.hh: Upgrade to 'clean-up'. * jardonnet/test/Makefile: Add rules. registration/final_qk.hh | 8 +- registration/icp.hh | 6 - registration/multiscale.hh | 78 +++++++++++++++++++ registration/quat7.hh | 2 registration/registration.hh | 171 +++++++++++++++++++++++++++++++++++++++++++ registration/tools.hh | 54 +++++-------- test/Makefile | 13 ++- test/icp.cc | 18 ++-- test/registration.cc | 110 +++++++++++++++++++++++++++ 9 files changed, 410 insertions(+), 50 deletions(-) Index: jardonnet/test/registration.cc --- jardonnet/test/registration.cc (revision 0) +++ jardonnet/test/registration.cc (revision 0) @@ -0,0 +1,110 @@ +#include <mln/core/image/image3d.hh> + +#include <mln/io/pbm/load.hh> +#include <mln/io/pbm/save.hh> +#include <mln/io/ppm/save.hh> +#include <mln/convert/to_p_array.hh> +#include <mln/norm/l2.hh> + +#include <sandbox/jardonnet/registration/icp.hh> +#include <sandbox/jardonnet/registration/tools.hh> +#include <sandbox/jardonnet/registration/final_qk.hh> +#include <mln/geom/bbox.hh> + +void usage(char *argv[]) +{ + std::cout << "usage : " << argv[0] + << " cloud.pbm surface.pbm q e" << std::endl + << " q >= 1 and e >= 1" << std::endl; + exit(1); +} + + +int main(int argc, char* argv[]) +{ + // usage + float q = std::atof(argv[3]); + int e = std::atoi(argv[4]); + if (argc != 5) + usage(argv); + if (q < 1 or e < 1) + usage(argv); + + using namespace mln; + typedef image2d< bool > image2db; + + image2db img1; + image2db img2; + + //load images + io::pbm::load(img1, argv[1]); + io::pbm::load(img2, argv[2]); + + //build p_arrays. + p_array<mln_psite_(image2db)> c = convert::to< p_array<point2d> >(img1); + p_array<mln_psite_(image2db)> x = convert::to< p_array<point2d> >(img2); + + //working box + const box<point2d> working_box = + enlarge(bigger(geom::bbox(c), geom::bbox(x)), 100); + + //Make a lazy_image map via function closest_point + closest_point<mln_psite_(image2db)> fun(x, working_box); + + // * Use real lazy image + //lazy_image< closest_point<mln_psite_(image2db)> > map(fun); + p_array<point2d> map; + quat7<2> qk ;//= registration::icp(c, map, q, e, x); + +#ifndef NDEBUG + std::cout << "closest_point(Ck[i]) = " << fun.i << std::endl; + std::cout << "Pts processed = " << registration::pts << std::endl; +#endif + + qk.apply_on(c, c, c.nsites()); + + float stddev, mean; + registration::mean_stddev(c, map, mean, stddev); + +#ifndef NDEBUG + std::cout << "mean : " << mean << std::endl; + std::cout << "stddev : " << stddev << std::endl; +#endif + + std::vector<float> length(c.nsites()); + for (size_t i = 0; i < c.nsites(); i++) + length[i] = norm::l2( convert::to< algebra::vec<2,float> >( c[i] - map(c[i]) ) ); + + + // final transform + quat7<2> fqk = registration::final_qk2(c, map, 2*stddev); + fqk.apply_on(c, c, c.nsites()); + + + //init output image + image2d<value::rgb8> output(convert::to_box2d(working_box), 0); + level::fill(output, literal::white); + + + //print x + for (unsigned i = 0; i < x.nsites(); i++) + { + //Xk points + point2d px(x[i][0], x[i][1]); + if (output.has(px)) + output(px) = literal::black; + } + + + for (unsigned i = 0; i < c.nsites(); i++) + { + //Ck points + point2d p(c[i][0], c[i][1]); + if (output.has(p)) + output(p) = literal::green; + } + + io::ppm::save(output, "registred.ppm"); + +} + Index: jardonnet/test/icp.cc --- jardonnet/test/icp.cc (revision 2432) +++ jardonnet/test/icp.cc (working copy) @@ -21,12 +21,11 @@ int main(int argc, char* argv[]) { - if (argc != 5) - usage(argv); - + // usage float q = std::atof(argv[3]); int e = std::atoi(argv[4]); - + if (argc != 5) + usage(argv); if (q < 1 or e < 1) usage(argv); @@ -45,15 +44,16 @@ I3d surface = convert::to_image3d(img2); //build p_arrays. - p_array<mln_point_(I3d)> c = convert::to_p_array(cloud); - p_array<mln_point_(I3d)> x = convert::to_p_array(surface); + p_array<mln_psite_(I3d)> c = convert::to_p_array(cloud); + p_array<mln_psite_(I3d)> x = convert::to_p_array(surface); //working box - const box_<mln_point_(I3d)> working_box = enlarge(bigger(c.bbox(),x.bbox()),100); + const box<mln_psite_(I3d)> working_box = + enlarge(bigger(c.bbox(),x.domain()),100); //Make a lazy_image map via function closest_point - closest_point<mln_point_(I3d)> fun(x, working_box); - lazy_image< closest_point<mln_point_(I3d)> > map(fun); + closest_point<mln_psite_(I3d)> fun(x, working_box); + lazy_image< closest_point<mln_psite_(I3d)> > map(fun); quat7<3> qk = registration::icp(c, map, q, e, x); Index: jardonnet/test/Makefile --- jardonnet/test/Makefile (revision 2432) +++ jardonnet/test/Makefile (working copy) @@ -13,18 +13,27 @@ gau: g++ gaussian.cc $(FLAG) -o 'gau' + +# # icp icpD: icp.cc +depend g++ icp.cc -I../../.. -g -o 'icpD' - icp: +depend g++ icp.cc -I../../.. -O3 -DNDEBUG -o 'icp' + +# # icp-ref icp_refD: icp_ref.cc ../registration/icp_ref.hh +depend g++ -Wall -W icp_ref.cc -I../../.. -g -o 'icp_refD' - icp_ref: icp_ref.cc ../registration/icp_ref.hh +depend g++ icp_ref.cc -I../../.. -O3 -DNDEBUG -o 'icp_ref' + +# # reg +regD: registration.cc +depend + g++ registration.cc -I../../.. -g -o 'reg' +reg: registration.cc +depend + g++ registration.cc -I../../.. -g -o 'reg' + bench: ruby bench.rb Index: jardonnet/registration/final_qk.hh --- jardonnet/registration/final_qk.hh (revision 2432) +++ jardonnet/registration/final_qk.hh (working copy) @@ -19,7 +19,7 @@ for (size_t i = 0; i < c.nsites(); i++) { - float f = norm::l2(algebra::vec<3,int> (c[i] - map(c[i]))); + float f = norm::l2(convert::to< algebra::vec<P::dim,int> > (c[i] - map(c[i]))); length[i] = f; mean += f; } @@ -80,8 +80,8 @@ float nb_point = 0; for (size_t i = 0; i < c.nsites(); ++i) { - algebra::vec<3,float> xki = map(c[i]); - algebra::vec<3,float> ci = c[i]; + algebra::vec<P::dim,float> xki = map(c[i]); + algebra::vec<P::dim,float> ci = c[i]; if (norm::l2(ci - xki) > nstddev) { @@ -94,7 +94,7 @@ mu_Xk /= nb_point; // qT - const algebra::vec<3,float> qT = mu_C - mu_Xk; + const algebra::vec<P::dim,float> qT = mu_C - mu_Xk; // qR quat7<P::dim> qk = final_qk(c, map, nstddev); Index: jardonnet/registration/save.hh Index: jardonnet/registration/tools.hh --- jardonnet/registration/tools.hh (revision 2432) +++ jardonnet/registration/tools.hh (working copy) @@ -12,14 +12,6 @@ namespace mln { - namespace registration - { - - - - } - - template <typename P> void shuffle(p_array<P>& a) { @@ -28,8 +20,8 @@ unsigned int r = rand() % a.nsites(); P tmp; tmp = a[i]; - a.hook_()[i] = a[r]; - a.hook_()[r] = tmp; + a[i] = a[r]; + a[r] = tmp; } } @@ -73,7 +65,7 @@ typedef algebra::vec<P::dim, float> input; typedef P result; - closest_point(const p_array<P>& X, const box_<P>& box) + closest_point(const p_array<P>& X, const box<P>& box) : X(X), box(box) #ifndef NDEBUG , i(0) @@ -105,13 +97,13 @@ return algebra::to_point<P>(best_x); } - const box_<P>& domain() const + const box<P>& domain() const { return box; } const p_array<P>& X; - const box_<P> box; + const box<P> box; #ifndef NDEBUG mutable unsigned i; @@ -161,10 +153,10 @@ template <typename P> inline - box_<P> //dif - enlarge(const box_<P>& box, unsigned b) + box<P> //dif + enlarge(const box<P>& box, unsigned b) { - box_<P> nbox(box); + mln::box<P> nbox(box); for (unsigned i = 0; i < P::dim; ++i) { @@ -175,7 +167,7 @@ } template <typename P> - box_<P> bigger(box_<P> a, box_<P> b) + box<P> bigger(const box<P>& a, const box<P>& b) { P pmin,pmax; @@ -185,7 +177,7 @@ pmax[i] = (a.pmax()[i] > b.pmax()[i]) ? a.pmax()[i] : b.pmax()[i]; } - return box_<P>(pmin, pmax); + return box<P>(pmin, pmax); } @@ -214,13 +206,13 @@ template < typename P > inline - box_<point2d> - to_box2d(const box_<P>& b) + box<point2d> + to_box2d(const box<P>& b) { point2d pmin(b.pmin()[0], b.pmin()[1]); point2d pmax(b.pmax()[0], b.pmax()[1]); - return box_<point2d>(pmin, pmax); + return box<point2d>(pmin, pmax); } @@ -246,17 +238,17 @@ //point3d -> point2d template <typename T> inline - point_<grid::square, T> - to_point2d(const point_<grid::cube, T>& p) + point<grid::square, T> + to_point2d(const point<grid::cube, T>& p) { - return point_<grid::square, T>(p[0], p[1]); + return point<grid::square, T>(p[0], p[1]); } //point2d -> point2d template <typename T> inline - point_<grid::square, T>& - to_point2d(const point_<grid::square, T>& p) + point<grid::square, T>& + to_point2d(const point<grid::square, T>& p) { return p; } @@ -264,16 +256,16 @@ //point2d -> point3d template <typename T> inline - point_<grid::cube, T> - to_point3d(const point_<grid::square, T>& p) + point<grid::cube, T> + to_point3d(const point<grid::square, T>& p) { - return point_<grid::cube, T>(p[0], p[1], 0); + return point<grid::cube, T>(p[0], p[1], 0); } //point3d -> point3d template <typename T> inline - point_<grid::cube, T>& - to_point3d(const point_<grid::cube, T>& p) + point<grid::cube, T>& + to_point3d(const point<grid::cube, T>& p) { return p; } Index: jardonnet/registration/quat7.hh --- jardonnet/registration/quat7.hh (revision 2432) +++ jardonnet/registration/quat7.hh (working copy) @@ -69,7 +69,7 @@ assert(_qR.is_unit()); for (size_t i = 0; i < c_length; i++) - output.hook_()[i] = algebra::to_point<P>((*this)(input[i])); + output[i] = algebra::to_point<P>((*this)(input[i])); } friend std::ostream& operator << (std::ostream& os, quat7& q) Index: jardonnet/registration/multiscale.hh --- jardonnet/registration/multiscale.hh (revision 0) +++ jardonnet/registration/multiscale.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_MULTISCALE_HH +# define MLN_REGISTRATION_MULTISCALE_HH + +# include <mln/registration/icp.hh> + +namespace mln +{ + + namespace registration + { + + // FIXME: Make it works in 3d *AND* 2d + template <typename P, typename M> + inline + quat7<P::dim> + multiscale(p_array<P>& cloud, + const M& map, + const float q, + const unsigned nb_it, + const p_array<P>& x) + { + trace::entering("registration::registration"); + + mln_precondition(P::dim == 3 || P::dim == 2); + mln_precondition(cloud.nsites() != 0); + + // Shuffle cloud + shuffle(cloud); + + //init rigid transform qk + quat7<P::dim> qk; + + //run registration + for (int e = nb_it-1; e >= 0; e--) + { + unsigned l = cloud.nsites() / std::pow(q, e); + l = (l<1) ? 1 : l; + impl::registration_(cloud, map, qk, l, 1e-3); + } + + trace::exiting("registration::registration"); + + return qk; + } + + } // end of namespace mln::registration + +} // end of namespace mln + + +#endif // ! MLN_REGISTRATION_MULTISCALE_HH Index: jardonnet/registration/icp_ref.hh Index: jardonnet/registration/registration.hh --- jardonnet/registration/registration.hh (revision 0) +++ jardonnet/registration/registration.hh (revision 0) @@ -0,0 +1,171 @@ +// 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_REGISTRATION_HH +# define MLN_REGISTRATION_REGISTRATION_HH + +/*! \file mln/registration/registration.hh + * + * \brief image registration + */ + +# include <iostream> +# include <string> +# include <cmath> + +# include <mln/algebra/quat.hh> +# include <mln/algebra/vec.hh> +# include <mln/make/w_window.hh> +# include <mln/make/w_window3d.hh> + +# include <mln/value/rgb8.hh> +# include <mln/literal/colors.hh> +# include <mln/literal/black.hh> +# include <mln/level/fill.hh> +# include <mln/io/ppm/save.hh> + + +# include "tools.hh" + +# include "cloud.hh" +# include "quat7.hh" +# include "update_qk.hh" +# include "chamfer.hh" + +# include "save.hh" + +namespace mln +{ + + namespace registration + { + +#ifndef NDEBUG + static unsigned pts = 0; +#endif + + /*! Registration FIXME : doxy + * + * + */ + template <typename I, typename J> + inline + mln_concrete(I) + registration(const Image<I>& cloud, + const Image<J>& surface); + +# ifndef MLN_INCLUDE_ONLY + + namespace impl + { + + template <typename P, typename M> + inline + void + registration_(const p_array<P>& C, + const M& map, + quat7<P::dim>& qk, + const unsigned c_length, + const float epsilon = 1e-3) + { + trace::entering("registration::impl::registration_"); + + buffer<4,quat7<P::dim> > buf_qk; + 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; + + buf_qk.store(qk); + + qk.apply_on(C, Ck, c_length); + + unsigned int k = 0; + do { + buf_dk.store(d_k); + + //compute qk + qk = match(C, mu_C, Ck, map, c_length); + buf_qk.store(qk); + + //update qk + if (k > 2) + qk = update_qk(buf_qk.get_array(), buf_dk.get_array()); + qk._qR.set_unit(); + buf_qk[0] = qk; + + //Ck+1 = qk(C) + qk.apply_on(C, Ck, c_length); + + // d_k = d(Yk, Pk+1) + // = d(closest(qk-1(P)), qk(P)) + d_k = rms(C, map, c_length, buf_qk[1], qk); + + k++; + } while ((qk - buf_qk[1]).sqr_norm() / qk.sqr_norm() > epsilon); + + trace::exiting("registration::impl::registration_"); + } + + } // end of namespace mln::registration::impl + + + // FIXME: Separate icp.hh registration.hh multiscale_registration.hh + // FIXME: Make it works in 3d *AND* 2d + template <typename P, typename M> + inline + quat7<P::dim> + registration(p_array<P>& cloud, + const M& map, + const p_array<P>& x) + { + trace::entering("registration::registration"); + + mln_precondition(P::dim == 3 || P::dim == 2); + mln_precondition(cloud.nsites() != 0); + + //init rigid transform qk + quat7<P::dim> qk; + + //run registration + impl::registration_(cloud, map, qk, l, 1e-3); + + trace::exiting("registration::registration"); + + return qk; + } + +# endif // ! MLN_INCLUDE_ONLY + + } // end of namespace mln::registration + +} // end of namespace mln + + +#endif // ! MLN_REGISTRATION_REGISTRATION_HH Index: jardonnet/registration/icp.hh --- jardonnet/registration/icp.hh (revision 2432) +++ jardonnet/registration/icp.hh (working copy) @@ -163,7 +163,7 @@ template <typename P, typename M> inline quat7<P::dim> - icp(p_array<P> cloud, // More efficient without reference + icp(p_array<P>& cloud, const M& map, const float q, const unsigned nb_it, @@ -182,7 +182,7 @@ #ifndef NDEBUG // FIXME: theo - const box_<P> working_box = enlarge(bigger(cloud.bbox(),x.bbox()),5); + const box<P> working_box = enlarge(bigger(geom::bbox(cloud),geom::bbox(x)),5); image2d<value::rgb8> tmp(convert::to_box2d(working_box), 1); level::fill(tmp, literal::black); //write X @@ -230,7 +230,7 @@ mln_piter(p_array<P>) p(cloud); for_all(p) { - algebra::vec<3,float> p_ = point3d(p), qp_ = qk(p_); + algebra::vec<P::dim,float> p_ = point3d(p), qp_ = qk(p_); point2d qp = point2d(int(qp_[0]), int(qp_[1])); if (tmp.has(qp)) tmp(qp) = c;