https://svn.lrde.epita.fr/svn/oln/trunk/milena/sandbox Index: ChangeLog from Ugo Jardonnet <jardonnet@lrde.epita.fr> Sandbox: ICP: icp_ref + cleanup. icp_ref uses no rounding. * jardonnet/test/icp_ref.cc: New. * jardonnet/registration/icp_ref.hh: New. * jardonnet/test/icp.cc: clean up. * jardonnet/test/Makefile: Update. * jardonnet/registration/final_qk.hh: Put final_qk function in a dedicated file. * jardonnet/registration/tools.hh: Add buffer and shuffle. * jardonnet/registration/icp.hh: Minor fix. registration/cloud.hh | 2 registration/final_qk.hh | 107 +++++++++++++++++++ registration/icp.hh | 9 - registration/icp_ref.hh | 254 +++++++++++++++++++++++++++++++++++++++++++++++ registration/quat7.hh | 2 registration/tools.hh | 116 +++++---------------- test/Makefile | 4 test/icp.cc | 12 +- test/icp_ref.cc | 87 ++++++++++++++++ 9 files changed, 495 insertions(+), 98 deletions(-) Index: jardonnet/test/icp_ref.cc --- jardonnet/test/icp_ref.cc (revision 0) +++ jardonnet/test/icp_ref.cc (revision 0) @@ -0,0 +1,87 @@ +#include <mln/core/image3d.hh> + +#include <mln/io/pbm/load.hh> +#include <mln/io/pbm/save.hh> +#include <mln/io/ppm/save.hh> +#include <mln/norm/l2.hh> + +#include <sandbox/jardonnet/registration/icp_ref.hh> +#include <sandbox/jardonnet/registration/tools.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[]) +{ + if (argc != 5) + usage(argv); + + float q = std::atof(argv[3]); + int e = std::atoi(argv[4]); + + if (q < 1 or e < 1) + usage(argv); + + using namespace mln; + typedef image3d< bool > I3d; + + image2d< bool > img1; + image2d< bool > img2; + + //load images + io::pbm::load(img1, argv[1]); + io::pbm::load(img2, argv[2]); + + //convert to image3d + I3d cloud = convert::to_image3d(img1); + 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); + + // FIXME : TODO : map : vec<3,float> -> point + + quat7<3> 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.npoints()); + + //init output image + //FIXME: Should be like + //mln_concrete(I) output(res.bbox()) = convert::to_image<I>(res) ? + + const box_<point2d> box2d(400,700); + image2d<value::rgb8> output(box2d, 1); + + //to 2d : projection (FIXME:if 3d) + for (size_t i = 0; i < c.npoints(); i++) + { + //Ck points + point2d p(c[i][0], c[i][1]); + if (output.has(p)) + output(p) = literal::white; + //Xk points + } + + for (size_t i = 0; i < x.npoints(); i++) + { + point2d x(map(c[i])[0], map(c[i])[1]); + if (output.has(x)) + output(x) = literal::green; + } + + io::ppm::save(output, "registred.ppm"); + +} + Index: jardonnet/test/icp.cc --- jardonnet/test/icp.cc (revision 1945) +++ jardonnet/test/icp.cc (working copy) @@ -7,6 +7,7 @@ #include <sandbox/jardonnet/registration/icp.hh> #include <sandbox/jardonnet/registration/tools.hh> +#include <sandbox/jardonnet/registration/final_qk.hh> void usage(char *argv[]) { @@ -57,18 +58,16 @@ std::cout << "Pts processed = " << registration::pts << std::endl; #endif - //FIXME: register img1 + qk.apply_on(c, c, c.npoints()); //init output image //FIXME: Should be like //mln_concrete(I) output(res.bbox()) = convert::to_image<I>(res) ? - qk.apply_on(c, c, c.npoints()); const box_<point2d> box2d(400,700); image2d<value::rgb8> output(box2d, 1); - float stddev, mean; registration::mean_stddev(c, map, mean, stddev); @@ -103,10 +102,15 @@ else output(p) = literal::white; } + } + + // print surface + for (size_t i = 0; i < c.npoints(); i++) + { //Xk points point2d x(map(c[i])[0], map(c[i])[1]); if (output.has(x)) - output(x) = value::rgb8(0,255,0); + output(x) = literal::green; } io::ppm::save(output, "registred.ppm"); Index: jardonnet/test/Makefile --- jardonnet/test/Makefile (revision 1945) +++ jardonnet/test/Makefile (working copy) @@ -19,6 +19,9 @@ icp: icp.cc +depend icp.o g++ icp.o -I../../.. -O3 -DNDEBUG -o 'icp' +icp_ref: + g++ icp_ref.cc -I../../.. -g -o 'icp_ref' + icp.o: g++ -c icp.cc -I../../.. -O3 -DNDEBUG @@ -30,6 +33,7 @@ rm -f sub gsub gau icpD icp rm -f log.dat registred.pbm rm -f i_* + rm -f tmp.ppm registred.ppm semantic.cache .PHONY: check bench icpD Index: jardonnet/registration/final_qk.hh --- jardonnet/registration/final_qk.hh (revision 0) +++ jardonnet/registration/final_qk.hh (revision 0) @@ -0,0 +1,107 @@ +#ifndef MLN_REGISTRATION_FINAL_QK_HH +# define MLN_REGISTRATION_FINAL_QK_HH + +namespace mln +{ + + namespace registration + { + + template <typename P, typename M> + void mean_stddev(const p_array<P>& c, + const M& map, + float& mean, + float& stddev) + { + //mean + length + std::vector<float> length(c.npoints()); + mean = 0; + + for (size_t i = 0; i < c.npoints(); i++) + { + float f = norm::l2(algebra::vec<3,int> (c[i] - map(c[i]))); + length[i] = f; + mean += f; + } + mean /= c.npoints(); + + //standar variation + stddev = 0; + for (size_t i = 0; i < c.npoints(); i++) + stddev += (length[i] - mean) * (length[i] - mean); + stddev /= c.npoints(); + stddev = math::sqrt(stddev); + } + + + //final qk : only use point less than nstddev (2*stddev); + template <typename P, typename M> + quat7<P::dim> + final_qk(const p_array<P>& c, + const M& map, + float nstddev) + { + p_array<P> newc; + algebra::vec<3,float> mu_newc(literal::zero); + + for (size_t i = 0; i < c.npoints(); ++i) + { + algebra::vec<3,float> xki = map(c[i]); + algebra::vec<3,float> ci = c[i]; + + if (norm::l2(ci - xki) > nstddev) + { + newc.append(c[i]); + mu_newc += ci; + } + } + mu_newc /= newc.npoints(); + + quat7<P::dim> qk = match(newc, mu_newc, newc, map, newc.npoints()); + + return qk; + } + + //final > nstddev for translation; < nstddev for rotation + template <typename P, typename M> + quat7<P::dim> + final_qk2(const p_array<P>& c, + const M& map, + float nstddev) + { + //mu_Xk = center map(Ck) + algebra::vec<3,float> mu_Xk(literal::zero); + algebra::vec<3,float> mu_C(literal::zero); + + float nb_point = 0; + for (size_t i = 0; i < c.npoints(); ++i) + { + algebra::vec<3,float> xki = map(c[i]); + algebra::vec<3,float> ci = c[i]; + + if (norm::l2(ci - xki) > nstddev) + { + mu_C += ci; + mu_Xk += xki; + nb_point++; + } + } + mu_C /= nb_point; + mu_Xk /= nb_point; + + // qT + const algebra::vec<3,float> qT = mu_C - mu_Xk; + + // qR + quat7<P::dim> qk = final_qk(c, map, nstddev); + qk._qT = qT; + + return qk; + } + + + } // end of namespace mln::registration + +} + +#endif // MLN_REGISTRATION_FINAL_QK_HH Index: jardonnet/registration/tools.hh --- jardonnet/registration/tools.hh (revision 1945) +++ jardonnet/registration/tools.hh (working copy) @@ -12,104 +12,46 @@ namespace mln { - namespace registration - { - - template <typename P, typename M> - void mean_stddev(const p_array<P>& c, - const M& map, - float& mean, - float& stddev) + template <typename P> + void shuffle(p_array<P>& a) { - //mean + length - std::vector<float> length(c.npoints()); - mean = 0; - - for (size_t i = 0; i < c.npoints(); i++) + for (unsigned int i = 0; i < a.npoints(); i++) { - float f = norm::l2(algebra::vec<3,int> (c[i] - map(c[i]))); - length[i] = f; - mean += f; + unsigned int r = rand() % a.npoints(); + P tmp; + tmp = a[i]; + a.hook_()[i] = a[r]; + a.hook_()[r] = tmp; } - mean /= c.npoints(); - - //standar variation - stddev = 0; - for (size_t i = 0; i < c.npoints(); i++) - stddev += (length[i] - mean) * (length[i] - mean); - stddev /= c.npoints(); - stddev = math::sqrt(stddev); } - - //final qk : only use point less than nstddev (2*stddev); - template <typename P, typename M> - quat7<P::dim> - final_qk(const p_array<P>& c, - const M& map, - float nstddev) + template <unsigned int n, typename T> + struct buffer { - p_array<P> newc; - algebra::vec<3,float> mu_newc(literal::zero); - - for (size_t i = 0; i < c.npoints(); ++i) - { - algebra::vec<3,float> xki = map(c[i]); - algebra::vec<3,float> ci = c[i]; - - if (norm::l2(ci - xki) > nstddev) + buffer() + : setted(0) { - newc.append(c[i]); - mu_newc += ci; } - } - mu_newc /= newc.npoints(); - - quat7<P::dim> qk = match(newc, mu_newc, newc, map, newc.npoints()); - return qk; - } - - //final > nstddev for translation; < nstddev for rotation - template <typename P, typename M> - quat7<P::dim> - final_qk2(const p_array<P>& c, - const M& map, - float nstddev) - { - //mu_Xk = center map(Ck) - algebra::vec<3,float> mu_Xk(literal::zero); - algebra::vec<3,float> mu_C(literal::zero); - - float nb_point = 0; - for (size_t i = 0; i < c.npoints(); ++i) + void add(T e) { - algebra::vec<3,float> xki = map(c[i]); - algebra::vec<3,float> ci = c[i]; + for (int i = 0; i < n-1; i++) + buf[i+1] = buf[i]; + buf[0] = e; - if (norm::l2(ci - xki) > nstddev) - { - mu_C += ci; - mu_Xk += xki; - nb_point++; - } + setted++; } - mu_C /= nb_point; - mu_Xk /= nb_point; - - // qT - const algebra::vec<3,float> qT = mu_C - mu_Xk; - // qR - quat7<P::dim> qk = final_qk(c, map, nstddev); - qk._qT = qT; - - return qk; + T& operator[](unsigned int i) + { + assert(i < n && i < setted); + return buf[i]; } - - } // end of namespace mln::registration - + private: + T buf[n]; + unsigned int setted; + }; //FIXME: groe length template <typename P> @@ -127,7 +69,7 @@ result //inline - operator () (const P& Ck) const + operator () (const input& Ck) const { #ifndef NDEBUG @@ -170,13 +112,13 @@ struct lazy_image { // Fun is potentially an image. - lazy_image(F& fun) + lazy_image(const F& fun) : value(fun.domain()), is_known(fun.domain()), fun(fun) { } // FIXME: hack, remove this constructor - lazy_image(F& fun, int nrows, int ncols, int nslis) + lazy_image(const F& fun, int nrows, int ncols, int nslis) : value(nrows, ncols,1), is_known(nrows,ncols,1), fun(fun) { } @@ -197,7 +139,7 @@ //FIXME: 3d -> //mln_dim(ml_input(input)) mutable image3d<mln_result(F)> value; mutable image3d<bool> is_known; - F& fun; + const F& fun; }; Index: jardonnet/registration/quat7.hh --- jardonnet/registration/quat7.hh (revision 1945) +++ jardonnet/registration/quat7.hh (working copy) @@ -126,7 +126,7 @@ quat7<P::dim> match(const p_array<P>& C, const algebra::vec<P::dim,float>& mu_C, const p_array<P>& Ck, - M& map, + const M& map, size_t c_length) { //mu_Xk = center map(Ck) Index: jardonnet/registration/cloud.hh --- jardonnet/registration/cloud.hh (revision 1945) +++ jardonnet/registration/cloud.hh (working copy) @@ -46,7 +46,7 @@ template <typename P, typename M> float rms(const p_array<P>& a1, quat7<P::dim>& qk, - M& map, + const M& map, const size_t length) { assert(length <= a1.npoints()); Index: jardonnet/registration/icp_ref.hh --- jardonnet/registration/icp_ref.hh (revision 0) +++ jardonnet/registration/icp_ref.hh (revision 0) @@ -0,0 +1,254 @@ +// 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_ICP_HH +# define MLN_REGISTRATION_ICP_HH + +/*! \file mln/registration/icp.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) + icp(const Image<I>& cloud, + const Image<J>& surface); + +# ifndef MLN_INCLUDE_ONLY + + namespace impl + { + + template <typename P, typename M> + inline + void + icp_(const p_array<P>& C, + const M& map, + quat7<P::dim>& qk, + const size_t c_length, + const float epsilon = 1e-3) + { + trace::entering("registration::impl::icp_"); + +#ifndef NDEBUG + //display registred points + std::cout << "Register " + << c_length << " points" << std::endl; + std::cout << "k\t\te_k >=\td_k\tdqk" << std::endl; +#endif + + quat7<P::dim> buf_qk[4]; + float buf_dk[3]; + + float err = 100; + //float err_bis; + p_array<P> Ck(C); + + algebra::vec<P::dim,float> mu_C = center(C, c_length), mu_Xk; + + buf_qk[0] = qk; + + qk.apply_on(C, Ck, c_length); + + unsigned int k = 0; + + do { + //update buff dk //FIXME: rewrite it + buf_dk[2] = buf_dk[1]; + buf_dk[1] = buf_dk[0]; + buf_dk[0] = err; + + //compute qk + qk = match(C, mu_C, Ck, map, c_length); + + //update buff qk //FIXME: rewrite it + buf_qk[3] = buf_qk[2]; + buf_qk[2] = buf_qk[1]; + buf_qk[1] = buf_qk[0]; + buf_qk[0] = qk; + + //update qk + /* + if (k > 3) + qk = update_qk(buf_qk, buf_dk); + qk._qR.set_unit(); + buf_qk[0] = qk; + */ + + //Ck+1 = qk(C) + qk.apply_on(C, Ck, c_length); + + // e_k = d(Yk, Pk) + // = d(closest(Pk), Pk) + // = d(closest(qk-1(P)), qk-1(P)) + float e_k = rms(C, map, c_length, buf_qk[1], buf_qk[1]); + + // d_k = d(Yk, Pk+1) + // = d(closest(qk-1(P)), qk(P)) + float d_k = rms(C, map, c_length, buf_qk[1], qk); + + + //err = d(Ck+1,Xk) = d_k + err = rms(C, qk, map, c_length); + + //err = d(Ck,Xk) = e_k + float err_bis = rms(C, buf_qk[1], map, c_length); + +#ifndef NDEBUG + //plot file + std::cout << k << '\t' << (e_k >= d_k ? ' ' : '-') << '\t' << e_k << '\t' << d_k << '\t' + << ((qk - buf_qk[1]).sqr_norm() / qk.sqr_norm()) << '\t' + << std::endl; + //count the number of points processed + pts += c_length; +#endif + k++; + } while ((qk - buf_qk[1]).sqr_norm() / qk.sqr_norm() > epsilon); + + trace::exiting("registration::impl::icp_"); + } + + } // end of namespace mln::registration::impl + + + // Only for 3d images + template <typename P, typename M> + inline + quat7<P::dim> + icp(p_array<P> cloud, // FIXME : is almost const (shuffled) + const M& map, + const float q, + const unsigned nb_it, + const p_array<P>& x) + { + trace::entering("registration::icp"); + + mln_precondition(P::dim == 3); + mln_precondition(cloud.npoints() != 0); + + shuffle(cloud); + + //init rigid transform qk + quat7<P::dim> qk; + +#ifndef NDEBUG // FIXME: theo + image2d<value::rgb8> tmp(500,800); + level::fill(tmp, literal::black); + //write X + mln_piter(p_array<P>) p(x); + for_all(p) + { + point2d qp = make::point2d(p[0], p[1]); + if (tmp.has(qp)) + tmp(qp) = literal::white; + } +#endif + + + //run icp + for (int e = nb_it-1; e >= 0; e--) + { + + size_t l = cloud.npoints() / std::pow(q, e); + l = (l<1) ? 1 : l; + impl::icp_(cloud, map, qk, l, 1e-3); + +#ifndef NDEBUG + { + value::rgb8 c; + switch (e) { + case 2: c = literal::green; break; + case 1: c = literal::blue; break; + case 0: c = literal::red; break; + } + mln_piter(p_array<P>) p(cloud); + for_all(p) + { + algebra::vec<3,float> p_ = point3d(p), qp_ = qk(p_); + point2d qp = make::point2d(int(qp_[0]), int(qp_[1])); + if (tmp.has(qp)) + tmp(qp) = c; + } + if (e == 0) + io::ppm::save(tmp, "tmp.ppm"); + } +#endif + } + + trace::exiting("registration::icp"); + + return qk; + } + +# endif // ! MLN_INCLUDE_ONLY + + } // end of namespace mln::registration + +} // end of namespace mln + + +#endif // ! MLN_REGISTRATION_ICP_HH Index: jardonnet/registration/icp.hh --- jardonnet/registration/icp.hh (revision 1945) +++ jardonnet/registration/icp.hh (working copy) @@ -85,9 +85,9 @@ template <typename P, typename M> inline - p_array<P> + void icp_(const p_array<P>& C, - M& map, + const M& map, quat7<P::dim>& qk, const size_t c_length, const float epsilon = 1e-3) @@ -171,7 +171,6 @@ } while (/*k < 3 ||*/ (qk - buf_qk[1]).sqr_norm() / qk.sqr_norm() > epsilon); trace::exiting("registration::impl::icp_"); - return Ck; } } // end of namespace mln::registration::impl @@ -181,8 +180,8 @@ template <typename P, typename M> inline quat7<P::dim> - icp(p_array<P> cloud, - M& map, + icp(p_array<P> cloud, //here reference implies low efficiency (FIXME:check again) + const M& map, const float q, const unsigned nb_it, const p_array<P>& x)