https://svn.lrde.epita.fr/svn/oln/trunk/milena/sandbox Index: ChangeLog from Ugo Jardonnet <ugo.jardonnet@lrde.epita.fr> Bench + Mean error varation problem. * jardonnet/test/icp.cc: Add printing. * jardonnet/test/bench.rb: Update plot. * jardonnet/test/plotscript: Auto update. * jardonnet/registration/save.hh: Save a p_array. * jardonnet/registration/cloud.hh: Fix rms. * jardonnet/registration/icp.hh: Update error computation. registration/cloud.hh | 28 ++++++++++++- registration/icp.hh | 103 +++++++++++++++++++++++++++++++++++--------------- registration/save.hh | 41 +++++++++++++++++++ test/bench.rb | 20 ++++++--- test/icp.cc | 11 +++-- test/plotscript | 8 +++ 6 files changed, 168 insertions(+), 43 deletions(-) Index: jardonnet/test/icp.cc --- jardonnet/test/icp.cc (revision 1924) +++ jardonnet/test/icp.cc (working copy) @@ -50,7 +50,7 @@ closest_point<mln_point_(I3d)> fun(x, box_<point3d>(1000,1000,1)); lazy_image< closest_point<mln_point_(I3d)> > map(fun); - quat7<3> qk = registration::icp(c, map, q, e); + quat7<3> qk = registration::icp(c, map, q, e, x); #ifndef NDEBUG std::cout << "closest_point(Ck[i]) = " << fun.i << std::endl; @@ -78,7 +78,10 @@ mean += f; } mean /= c.npoints(); - std::cout << mean << std::endl; +#ifndef NDEBUG + std::cout << "mean : " << mean << std::endl; +#endif + //standar variation float stdev = 0; @@ -86,7 +89,9 @@ stdev += (length[i] - mean) * (length[i] - mean); stdev /= c.npoints(); stdev = math::sqrt(stdev); - std::cout << stdev << std::endl; +#ifndef NDEBUG + std::cout << "stddev : " << stdev << std::endl; +#endif //final translate translate using point only separated less than 2*stdev //mu_Xk = center map(Ck) Index: jardonnet/test/bench.rb --- jardonnet/test/bench.rb (revision 1924) +++ jardonnet/test/bench.rb (working copy) @@ -20,14 +20,20 @@ plot_qe.close -plotscipt = File.new("plotscript", "w+") -plotscipt.puts "set xlabel \"q\"" -plotscipt.puts "set ylabel \"e\"" -plotscipt.puts "set zlabel \"time\"" -plotscipt.puts "splot \"log.dat\"" -plotscipt.puts "pause -1" -plotscipt.close +plotscript = File.new("plotscript", "w+") +plotscript.puts "set xlabel \"q\"" +plotscript.puts "set ylabel \"e\"" +plotscript.puts "set zlabel \"time\"" +plotscript.puts "splot \"log.dat\"" +plotscript.puts "set terminal png nocrop enhanced size 800,600" +plotscript.puts "set output \"bench_qe.png\"" +plotscript.puts "replot" +plotscript.puts "set output" +plotscript.puts "set terminal x11" +plotscript.close exec("gnuplot plotscript") + + Index: jardonnet/test/plotscript --- jardonnet/test/plotscript (revision 1924) +++ jardonnet/test/plotscript (working copy) @@ -1,5 +1,11 @@ +set title "Bench : q, e" + set xlabel "q" set ylabel "e" set zlabel "time" + splot "log.dat" -pause -1 + +save "plot.gp" + +#pause(-1) \ No newline at end of file Index: jardonnet/registration/save.hh --- jardonnet/registration/save.hh (revision 0) +++ jardonnet/registration/save.hh (revision 0) @@ -0,0 +1,41 @@ +#ifndef MLN_REGISTRATION_SAVE_HH +# define MLN_REGISTRATION_SAVE_HH + +#include <string> + +namespace mln +{ + + namespace registration + { + + template <typename P> + void save_(const p_array<P>& Ck, int l) + { + static int id = 0; + + using namespace std; + + //FIXME: Should use Ck box but Ck.bbox() is not correct for c_length + image2d<bool> img(box2d(500,800), 0); + for (size_t i = 0; i < l; i++) + { + point2d p = convert::to_point2d(Ck[i]); + if (img.has(p)) + img(p) = true; + } + + //FIXME: Good but put point after c_length + //image2d<bool> img = convert::to_image2d(Ck); + + std::stringstream oss; + oss << "i_" << id++ << ".pbm"; + io::pbm::save(img, oss.str()); + + } + + } + +} + +#endif Index: jardonnet/registration/cloud.hh --- jardonnet/registration/cloud.hh (revision 1924) +++ jardonnet/registration/cloud.hh (working copy) @@ -11,6 +11,8 @@ # include <mln/core/p_array.hh> # include <mln/norm/l2.hh> +#include "quat7.hh" + namespace mln { @@ -43,6 +45,7 @@ template <typename P, typename M> float rms(const p_array<P>& a1, + quat7<P::dim>& qk, M& map, const size_t length) { @@ -51,11 +54,30 @@ float f = 0.f; for (size_t i = 0; i < length; ++i) { - algebra::vec<3,float> a1f = a1[i]; - algebra::vec<3,float> a2f = map(a1[i]); + algebra::vec<3,float> a1f = qk(a1[i]); + algebra::vec<3,float> a2f = map(algebra::to_point<P>(qk(a1[i]))); + f += norm::l2(a1f - a2f); + } + return f / length; + } + + template <typename P, typename M> + float rms(const p_array<P>& a1, + M& map, + const size_t length, + quat7<P::dim>& q1, + quat7<P::dim>& q2) + { + assert(length <= a1.npoints()); + + float f = 0.f; + for (size_t i = 0; i < length; ++i) + { + algebra::vec<3,float> a2f = q2(a1[i]); + algebra::vec<3,float> a1f = map(algebra::to_point<P>(q1(a1[i]))); f += norm::l2(a1f - a2f); } - return f / a1.npoints(); + return f / length; } } // end of namespace registration Index: jardonnet/registration/icp.hh --- jardonnet/registration/icp.hh (revision 1924) +++ jardonnet/registration/icp.hh (working copy) @@ -42,6 +42,13 @@ # 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" @@ -49,6 +56,8 @@ # include "update_qk.hh" # include "chamfer.hh" +# include "save.hh" + namespace mln { @@ -89,7 +98,7 @@ //display registred points std::cout << "Register " << c_length << " points" << std::endl; - std::cout << "k\terror\tdqk" << std::endl; + std::cout << "k\t\te_k >=\td_k\tdqk" << std::endl; #endif quat7<P::dim> buf_qk[4]; @@ -101,6 +110,8 @@ 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; @@ -120,49 +131,44 @@ 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); - //err = d(Ck+1,Xk) - err = rms(Ck, map, 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]); -#ifndef NDEBUG - { - /* - using namespace std; - //FIXME: Should use Ck box but Ck.bbox() is not correct for c_length - image2d<bool> img(box2d(500,800), 0); - for (size_t i = 0; i < c_length; i++) - { - point2d p = convert::to_point2d(Ck[i]); - if (img.has(p)) - img(p) = true; - } + // 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); - //FIXME: Good but put point after c_length - //image2d<bool> img = convert::to_image2d(Ck); - */ - stringstream oss; - static int pimp = 0; - oss << "i_" << pimp++ << ".pbm"; - io::pbm::save(img, oss.str()); - } + //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' << err << '\t' - << (qk - buf_qk[1]).sqr_norm() << '\t' + 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 (k < 3 || (qk - buf_qk[1]).sqr_norm() > epsilon); + } while (/*k < 3 ||*/ (qk - buf_qk[1]).sqr_norm() / qk.sqr_norm() > epsilon); trace::exiting("registration::impl::icp_"); return Ck; @@ -178,7 +184,8 @@ icp(p_array<P> cloud, M& map, const float q, - const unsigned nb_it) + const unsigned nb_it, + const p_array<P>& x) { trace::entering("registration::icp"); @@ -198,12 +205,50 @@ //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); // 3 is arbitrary fixed + + size_t l = cloud.npoints() / std::pow(q, e); l = (l<1) ? 1 : l; - impl::icp_(cloud, map, qk, l, e + 1e-1); + 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");