https://svn.lrde.epita.fr/svn/oln/trunk/milena/sandbox Index: ChangeLog from Ugo Jardonnet <jardonnet@lrde.epita.fr> Sandbox: ICP: Add material for slides illustration. * jardonnet/test/icp_ref.cc: Update. Mark centers of mass. * jardonnet/registration/save.hh: Update. Mark center of mass and orientation. * jardonnet/registration/power_it.hh: Update. Now works in n-D. * jardonnet/registration/quat7.hh: Update. Cosmetic change. * jardonnet/TODO: Update. * jardonnet/test/Makefile: Fix clean rule. TODO | 2 -- registration/icp_ref.hh | 6 ++---- registration/power_it.hh | 20 +++++++++----------- registration/quat7.hh | 5 ++--- registration/save.hh | 26 ++++++++++++++++++++++++-- test/Makefile | 5 +++-- test/icp_ref.cc | 31 ++++++++++++++++--------------- 7 files changed, 56 insertions(+), 39 deletions(-) Index: jardonnet/test/icp_ref.cc --- jardonnet/test/icp_ref.cc (revision 2031) +++ jardonnet/test/icp_ref.cc (working copy) @@ -43,27 +43,20 @@ I3d surface = convert::to_image3d(img2); //build p_arrays. - p_array< point_<grid::cube, float> > c; + typedef point_<grid::cube,float> point3df; + p_array<point3df> c; { mln_piter_(I3d) p(cloud.domain()); for_all(p) if (cloud(p)) - { - point_<grid::cube,float> p_; - p_[0] = p[0]; p_[1] = p[1]; p_[2] = p[2]; - c.append(p_); - } + c.append(algebra::to_point<point3df>(point3d(p))); } - p_array< point_<grid::cube, float> > x; + p_array<point3df> x; { mln_piter_(I3d) p(surface.domain()); for_all(p) if (surface(p)) - { - point_<grid::cube,float> p_; - p_[0] = p[0]; p_[1] = p[1]; p_[2] = p[2]; - x.append(p_); - } + x.append(algebra::to_point<point3df>(point3d(p))); } //working box @@ -84,20 +77,28 @@ image2d<value::rgb8> output(convert::to_box2d(working_box), 1); level::fill(output, literal::white); + //plot mu_Ck + point3df mu_Ck = registration::center(c, c.npoints()); + draw::plot(output, point2d(mu_Ck[0], mu_Ck[1]), literal::green); + + //plot mu_X + point3df mu_X = registration::center(x, x.npoints()); + draw::plot(output, point2d(mu_X[0], mu_X[1]), literal::black); + //to 2d : projection (FIXME:if 3d) for (unsigned i = 0; i < c.npoints(); i++) { point2d p(c[i][0], c[i][1]); if (output.has(p)) - output(p) = literal::black; + output(p) = literal::green; } - //ref image + //ref image (black) for (unsigned i = 0; i < x.npoints(); i++) { point2d px(x[i][0], x[i][1]); if (output.has(px)) - output(px) = literal::green; + output(px) = literal::black; } io::ppm::save(output, "registred.ppm"); Index: jardonnet/test/Makefile --- jardonnet/test/Makefile (revision 2031) +++ jardonnet/test/Makefile (working copy) @@ -36,12 +36,13 @@ clean: rm -f -- ./bin/* - rm -f sub gsub gau icpD icp icp_refD icp_ref + rm -f sub gsub gau icpD icp icp_refD icp_ref *.o rm -f log.dat registred.pbm rm -f i_* rm -f tmp.ppm registred.ppm semantic.cache + rm -f step_* -.PHONY: check bench test icpD +.PHONY: check bench test icpD icp_refD icp icp_ref +depend: g++ -M icp.cc $(FLAG) > +depend Index: jardonnet/TODO --- jardonnet/TODO (revision 2031) +++ jardonnet/TODO (working copy) @@ -12,8 +12,6 @@ adapt test for threshold (old thresholding) - - -La translation est mauvaise car certain point de Xk sont compté plusieurs fois. /bof - - - write trans : vec --> quat @@ -21,3 +19,15 @@ - - io/all.hh cassé \ No newline at end of file + +- - +Mettre au clair center, mean ... + +- - + +algebra::to_point<P> ne devrait pas etre dans algebra puisqu'il + est utile pour passer d'un point<int> vers point<float> +move it in convert + +- - +ecrire cov.hh. quoi en argument (p_array, std::set, std::vector?) \ No newline at end of file Index: jardonnet/registration/power_it.hh --- jardonnet/registration/power_it.hh (revision 2031) +++ jardonnet/registration/power_it.hh (working copy) @@ -12,29 +12,27 @@ namespace mln { - algebra::quat power_it(algebra::mat<4,4,float> A) + template <uint n> + algebra::vec<n,float> power_it(algebra::mat<n,n,float> A) { float normex = 1.; - algebra::vec<4,float> x0(literal::zero); - algebra::vec<4,float> b(literal::zero); + algebra::vec<n,float> x0(literal::zero); + algebra::vec<n,float> b(literal::zero); - algebra::vec<4,float> x(literal::zero); - for (int i = 0; i < 4; i++) + algebra::vec<n,float> x(literal::zero); + for (int i = 0; i < n; i++) x[i] = 1.; - while (fabs(norm::l2(x) - norm::l2(x0)) > 1e-6) + //FIXME: infinit loop. + while (about_equal(norm::l2(x),norm::l2(x0))) { x0 = x; normex = norm::l2(x); b = x / normex; x = A * b; } - normex = norm::l2(x); - - algebra::quat q(x / normex); - q.set_unit(); - return q; + return x.normalize(); } } Index: jardonnet/registration/save.hh --- jardonnet/registration/save.hh (revision 2031) +++ jardonnet/registration/save.hh (working copy) @@ -4,10 +4,12 @@ # include <mln/convert/to_image.hh> # include <mln/io/ppm/save.hh> # include <mln/io/pbm/save.hh> +# include <mln/draw/all.hh> # include <string> # include "quat7.hh" # include "tools.hh" +# include "power_it.hh" namespace mln { @@ -21,7 +23,7 @@ { mln_precondition(P::dim == 2); - image2d out = convert::to_image(a); + image2d<bool> out = convert::to_image(a); save(out, filename); } } @@ -51,7 +53,27 @@ image2d<value::rgb8> out(convert::to_box2d(working_box), 1); level::fill(out, literal::white); - //x in green + //plot mu_Ck + algebra::vec<P::dim,float> mu_Ck = center(ck, ck.npoints()); + draw::plot(out, point2d(mu_Ck[0], mu_Ck[1]), literal::green); + //shape orientation + algebra::mat<P::dim,P::dim,float> Mk(literal::zero); + for (unsigned i = 0; i < ck.npoints(); ++i) + { + algebra::vec<P::dim,float> Cki = ck[i]; + Mk += make::mat(Cki - mu_Ck) * trans(make::mat(Cki - mu_Ck)); + } + Mk /= c.npoints(); + algebra::vec<3,float> vck = power_it(Mk); + draw::line(out, point2d(mu_Ck[0], mu_Ck[1]), + point2d(mu_Ck[0]+vck[0]*10, mu_Ck[1]+vck[1]*10), + literal::red); + + //plot mu_X + P mu_X = center(x, x.npoints()); + draw::plot(out, point2d(mu_X[0], mu_X[1]), literal::black); + + //ck in green for (unsigned i = 0; i < ck.npoints(); i++) { point2d p(ck[i][0], ck[i][1]); Index: jardonnet/registration/quat7.hh --- jardonnet/registration/quat7.hh (revision 2031) +++ jardonnet/registration/quat7.hh (working copy) @@ -131,17 +131,16 @@ { //mu_Xk = center map(Ck) algebra::vec<P::dim,float> mu_Xk(literal::zero); - for (size_t i = 0; i < c_length; ++i) + for (unsigned i = 0; i < c_length; ++i) { algebra::vec<P::dim,float> xki = map(Ck[i]); mu_Xk += xki; } mu_Xk /= c_length; - // qR algebra::mat<P::dim,P::dim,float> Mk(literal::zero); - for (size_t i = 0; i < c_length; ++i) + for (unsigned i = 0; i < c_length; ++i) { algebra::vec<P::dim,float> Ci = C[i]; algebra::vec<P::dim,float> Xki = map(Ck[i]); Index: jardonnet/registration/icp_ref.hh --- jardonnet/registration/icp_ref.hh (revision 2031) +++ jardonnet/registration/icp_ref.hh (working copy) @@ -62,8 +62,6 @@ namespace mln { - - namespace registration { @@ -128,6 +126,7 @@ qk.apply_on(C, Ck, c_length); d_k_1 = d_k; + // d_k = d(Yk, Pk+1) // = d(closest(qk-1(P)), qk(P)) d_k = rms(C, map, c_length, buf_qk[1], qk); @@ -136,8 +135,7 @@ e_k = rms(C, map, c_length, buf_qk[1], buf_qk[1]); #ifndef NDEBUG - //save file - save_(qk,C,X,5); + save_(qk,C,X,2); //print info 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'