https://svn.lrde.epita.fr/svn/oln/trunk/milena/sandbox Index: ChangeLog from Ugo Jardonnet <jardonnet@lrde.epita.fr> Sandbox: ICP: Materials for report. Add/Update scripts: * jardonnet/test/bench.rb: Minor fix. * jardonnet/test/test.rb: New: Bench times. * jardonnet/test/plotscript: Auto Update. * jardonnet/test/script_latex.plot: Latex output. * jardonnet/test/Makefile: Add test rule. Add few images to the test set: * jardonnet/test/img/c4.pbm, * jardonnet/test/img/c6.pbm, * jardonnet/test/img/c7.pbm, * jardonnet/test/img/c8.pbm, * jardonnet/test/img/c9.pbm, * jardonnet/test/img/x3.pbm, * jardonnet/test/img/x6.pbm, * jardonnet/test/img/x7.pbm, * jardonnet/test/img/x8.pbm, * jardonnet/test/img/x9.pbm: New. Update test set: * jardonnet/test/img/01.pbm: Rename as ... * jardonnet/test/img/c0.pbm ... this. * jardonnet/test/img/02.pbm: Rename as ... * jardonnet/test/img/x0.pbm: ... this. Update code so as to produce report's results. * jardonnet/test/icp_ref.cc: Modify output image. * jardonnet/test/icp.cc: Modify output image. * jardonnet/registration/final_qk.hh: Cleanup. * jardonnet/registration/interpolation.hh: Update (const). * jardonnet/registration/tools.hh: Fix buffer. * jardonnet/registration/cloud.hh: Cleanup. * jardonnet/registration/icp_ref.hh: Fix. * jardonnet/registration/icp.hh: Minor Fix, Make use of buffer. * jardonnet/registration/update_qk.hh: Cleanup code. * jardonnet/registration/icp_map.hh: Remove. registration/cloud.hh | 23 ++++++++++++++++++ registration/final_qk.hh | 2 + registration/icp.hh | 47 +++++++++++++------------------------- registration/icp_ref.hh | 50 +++++++++-------------------------------- registration/interpolation.hh | 2 - registration/tools.hh | 8 ++++-- registration/update_qk.hh | 9 +++---- test/Makefile | 16 +++++++++---- test/bench.rb | 2 - test/icp.cc | 26 ++++++++++----------- test/icp_ref.cc | 23 +++++++++--------- test/plotscript | 3 -- test/script_latex.plot | 8 ++++++ test/test.rb | 51 ++++++++++++++++++++++++++++++++++++++++++ 14 files changed, 157 insertions(+), 113 deletions(-) Index: jardonnet/test/bench.rb --- jardonnet/test/bench.rb (revision 1978) +++ jardonnet/test/bench.rb (working copy) @@ -8,7 +8,7 @@ for e in 1..10 do t1 = Time.new - system("./icp 01.pbm 02.pbm #{q} #{e}") + system("./icp img/01.pbm img/02.pbm #{q} #{e}") t2 = Time.new print "#{q} #{e} #{t2-t1}\n" Index: jardonnet/test/test.rb --- jardonnet/test/test.rb (revision 0) +++ jardonnet/test/test.rb (revision 0) @@ -0,0 +1,51 @@ +#!/usr/bin/ruby + +print "# icp_ref \t icp\n" +print "-------------------------\n" + +q = 1 +e = 1 + +for i in 0..9 do + + # icp_ref + + t1 = Time.new + system("./icp_ref img/c#{i}.pbm img/x#{i}.pbm #{q} #{e}") + t2 = Time.new + t = t2 - t1 + + ta1 = Time.new + system("./icp_ref img/c#{i}.pbm img/x#{i}.pbm #{q} #{e}") + ta2 = Time.new + ta = ta2 - ta1 + + tb1 = Time.new + system("./icp_ref img/c#{i}.pbm img/x#{i}.pbm #{q} #{e}") + tb2 = Time.new + tb = tb2 - tb1 + + # icp + + tt1 = Time.new + system("./icp img/c#{i}.pbm img/x#{i}.pbm #{q} #{e}") + tt2 = Time.new + tt = tt2 - tt1 + + tta1 = Time.new + system("./icp img/c#{i}.pbm img/x#{i}.pbm #{q} #{e}") + tta2 = Time.new + tta = tta2 - tta1 + + ttb1 = Time.new + system("./icp img/c#{i}.pbm img/x#{i}.pbm #{q} #{e}") + ttb2 = Time.new + ttb = ttb2 - ttb1 + + # mean + + meant = (t + ta + tb) / 3 + meantt = (tt + tta + ttb) /3 + + print "#{i} #{t} \t #{tt}\n" +end Index: jardonnet/test/icp_ref.cc --- jardonnet/test/icp_ref.cc (revision 1978) +++ jardonnet/test/icp_ref.cc (working copy) @@ -25,6 +25,7 @@ float q = std::atof(argv[3]); int e = std::atoi(argv[4]); + if (q < 1 or e < 1) usage(argv); @@ -46,39 +47,37 @@ p_array<mln_point_(I3d)> c = convert::to_p_array(cloud); p_array<mln_point_(I3d)> x = convert::to_p_array(surface); + //working box + const box_<mln_point_(I3d)> working_box = enlarge(bigger(c.bbox(),x.bbox()),100); + // FIXME : TODO : map : vec<3,float> -> point + closest_point<mln_point_(I3d)> map(x, working_box); quat7<3> qk = registration::icp(c, map, q, e, x); #ifndef NDEBUG - std::cout << "closest_point(Ck[i]) = " << fun.i << std::endl; + std::cout << "closest_point(Ck[i]) = " << map.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); + image2d<value::rgb8> output(convert::to_box2d(working_box), 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 } + //ref image 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; + point2d px(x[i][0], x[i][1]); + if (output.has(px)) + output(px) = literal::green; } io::ppm::save(output, "registred.ppm"); Index: jardonnet/test/icp.cc --- jardonnet/test/icp.cc (revision 1978) +++ jardonnet/test/icp.cc (working copy) @@ -64,7 +64,8 @@ qk.apply_on(c, c, c.npoints()); //init output image - image2d<value::rgb8> output(convert::to_box2d(working_box), 1); + image2d<value::rgb8> output(convert::to_box2d(working_box), 0); + level::fill(output, literal::white); float stddev, mean; registration::mean_stddev(c, map, mean, stddev); @@ -79,10 +80,18 @@ length[i] = norm::l2(algebra::vec<3,int> (c[i] - map(c[i]))); // final transform - quat7<3> fqk = registration::final_qk(c, map, 2 * stddev); - std::cout << fqk << std::endl; + quat7<3> fqk = registration::final_qk2(c, map, 2*stddev); fqk.apply_on(c, c, c.npoints()); + //print x + for (size_t i = 0; i < x.npoints(); i++) + { + //Xk points + point2d px(x[i][0], x[i][1]); + if (output.has(px)) + output(px) = literal::green; + } + //to 2d : projection (FIXME:if 3d) for (size_t i = 0; i < c.npoints(); i++) { @@ -98,17 +107,8 @@ else if (length[i] > stddev) output(p) = value::rgb8(255,200,0); else - output(p) = literal::white; - } + output(p) = literal::black; } - - // 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) = literal::green; } io::ppm::save(output, "registred.ppm"); Index: jardonnet/test/plotscript --- jardonnet/test/plotscript (revision 1978) +++ jardonnet/test/plotscript (working copy) @@ -1,10 +1,7 @@ set xlabel "q" set ylabel "e" set zlabel "time" - splot "log.dat" - - set terminal png nocrop enhanced size 800,600 set output "bench_qe.png" replot Index: jardonnet/test/img/c4.pbm Cannot display: file marked as a binary type. svn:mime-type = application/octet-stream Index: jardonnet/test/img/c6.pbm Cannot display: file marked as a binary type. svn:mime-type = application/octet-stream Property changes on: jardonnet/test/img/c6.pbm ___________________________________________________________________ Name: svn:mime-type + application/octet-stream Index: jardonnet/test/img/c7.pbm Cannot display: file marked as a binary type. svn:mime-type = application/octet-stream Property changes on: jardonnet/test/img/c7.pbm ___________________________________________________________________ Name: svn:mime-type + application/octet-stream Index: jardonnet/test/img/c8.pbm Cannot display: file marked as a binary type. svn:mime-type = application/octet-stream Property changes on: jardonnet/test/img/c8.pbm ___________________________________________________________________ Name: svn:mime-type + application/octet-stream Index: jardonnet/test/img/c9.pbm Cannot display: file marked as a binary type. svn:mime-type = application/octet-stream Property changes on: jardonnet/test/img/c9.pbm ___________________________________________________________________ Name: svn:mime-type + application/octet-stream Index: jardonnet/test/img/x3.pbm Cannot display: file marked as a binary type. svn:mime-type = application/octet-stream Index: jardonnet/test/img/x6.pbm Cannot display: file marked as a binary type. svn:mime-type = application/octet-stream Property changes on: jardonnet/test/img/x6.pbm ___________________________________________________________________ Name: svn:mime-type + application/octet-stream Index: jardonnet/test/img/x7.pbm Cannot display: file marked as a binary type. svn:mime-type = application/octet-stream Property changes on: jardonnet/test/img/x7.pbm ___________________________________________________________________ Name: svn:mime-type + application/octet-stream Index: jardonnet/test/img/x8.pbm Cannot display: file marked as a binary type. svn:mime-type = application/octet-stream Property changes on: jardonnet/test/img/x8.pbm ___________________________________________________________________ Name: svn:mime-type + application/octet-stream Index: jardonnet/test/img/x9.pbm Cannot display: file marked as a binary type. svn:mime-type = application/octet-stream Property changes on: jardonnet/test/img/x9.pbm ___________________________________________________________________ Name: svn:mime-type + application/octet-stream Index: jardonnet/test/Makefile --- jardonnet/test/Makefile (revision 1978) +++ jardonnet/test/Makefile (working copy) @@ -16,11 +16,14 @@ icpD: icp.cc +depend g++ icp.cc -I../../.. -g -o 'icpD' -icp: icp.cc +depend icp.o +icp: icp.o +depend g++ icp.o -I../../.. -O3 -DNDEBUG -o 'icp' -icp_ref: - g++ icp_ref.cc -I../../.. -g -o 'icp_ref' +icp_refD: icp_ref.cc ../registration/icp_ref.hh +depend + g++ icp_ref.cc -I../../.. -g -o 'icp_refD' + +icp_ref: icp_ref.cc ../registration/icp_ref.hh +depend + g++ icp_ref.cc -I../../.. -pg -O3 -DNDEBUG -o 'icp_ref' icp.o: g++ -c icp.cc -I../../.. -O3 -DNDEBUG @@ -28,14 +31,17 @@ bench: ruby bench.rb +test: icp icp_ref + ruby test.rb + clean: rm -f -- ./bin/* - rm -f sub gsub gau icpD icp + rm -f sub gsub gau icpD icp icp_refD icp_ref rm -f log.dat registred.pbm rm -f i_* rm -f tmp.ppm registred.ppm semantic.cache -.PHONY: check bench icpD +.PHONY: check bench test icpD +depend: g++ -M icp.cc $(FLAG) > +depend Index: jardonnet/test/script_latex.plot --- jardonnet/test/script_latex.plot (revision 0) +++ jardonnet/test/script_latex.plot (revision 0) @@ -0,0 +1,8 @@ +set terminal latex +set output "eg1.tex" + +set xlabel "q" +set ylabel "e" +set zlabel "time" +splot "log.dat" + Index: jardonnet/registration/final_qk.hh --- jardonnet/registration/final_qk.hh (revision 1978) +++ jardonnet/registration/final_qk.hh (working copy) @@ -55,6 +55,8 @@ mu_newc += ci; } } + if (newc.npoints() == 0) + return quat7<P::dim>(); mu_newc /= newc.npoints(); quat7<P::dim> qk = match(newc, mu_newc, newc, map, newc.npoints()); Index: jardonnet/registration/interpolation.hh --- jardonnet/registration/interpolation.hh (revision 1978) +++ jardonnet/registration/interpolation.hh (working copy) @@ -5,7 +5,7 @@ namespace mln { - void polcoe(float x[], float y[], int n, + void polcoe(const float x[], const float y[], int n, float cof[]) { int k,j,i; Index: jardonnet/registration/tools.hh --- jardonnet/registration/tools.hh (revision 1978) +++ jardonnet/registration/tools.hh (working copy) @@ -33,7 +33,7 @@ { } - void add(T e) + void store(T e) { for (int i = 0; i < n-1; i++) buf[i+1] = buf[i]; @@ -48,6 +48,11 @@ return buf[i]; } + const T * get_array() + { + return buf; + } + private: T buf[n]; unsigned int setted; @@ -122,7 +127,6 @@ : value(nrows, ncols,1), is_known(nrows,ncols,1), fun(fun) { } - // FIXME: gore length const mln_result(F) //inline operator () (const typename F::input& p) const Index: jardonnet/registration/cloud.hh --- jardonnet/registration/cloud.hh (revision 1978) +++ jardonnet/registration/cloud.hh (working copy) @@ -16,6 +16,26 @@ namespace mln { + namespace geom + { + template <typename P> + P center(const p_array<P>& a) + { + if (a.npoints() == 0) + return P(); + + algebra::vec<P::dim,float> c(literal::zero); + for (unsigned i = 0; i < a.npoints(); ++i) + { + // FIXME : Ugly. + algebra::vec<P::dim,float> ai = a[i]; + c += ai; + } + + return algebra::to_point<P>(c / a.npoints()); + } + } + namespace registration { @@ -23,8 +43,9 @@ P center(const p_array<P>& a, size_t length) { algebra::vec<P::dim,float> c(literal::zero); - for (size_t i = 0; i < length; ++i) + for (unsigned i = 0; i < length; ++i) { + // FIXME : Ugly. algebra::vec<P::dim,float> ai = a[i]; c += ai; } Index: jardonnet/registration/icp_ref.hh --- jardonnet/registration/icp_ref.hh (revision 1978) +++ jardonnet/registration/icp_ref.hh (working copy) @@ -48,7 +48,6 @@ # include <mln/level/fill.hh> # include <mln/io/ppm/save.hh> - # include "tools.hh" # include "cloud.hh" @@ -101,64 +100,37 @@ std::cout << "k\t\te_k >=\td_k\tdqk" << std::endl; #endif - quat7<P::dim> buf_qk[4]; - float buf_dk[3]; + buffer<4,quat7<P::dim> > buf_qk; - float err = 100; - //float err_bis; - p_array<P> Ck(C); + p_array<P> Ck(C), Xk(C); + float d_k = 1000, d_k_1 = 1000; algebra::vec<P::dim,float> mu_C = center(C, c_length), mu_Xk; - buf_qk[0] = qk; + buf_qk.store(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; - */ + buf_qk.store(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_1 = d_k; // 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); + d_k = rms(C, map, c_length, buf_qk[1], qk); #ifndef NDEBUG + // e_k = d(closest(qk-1(P)), qk-1(P)) + float e_k = rms(C, map, c_length, buf_qk[1], buf_qk[1]); + //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' @@ -167,7 +139,7 @@ pts += c_length; #endif k++; - } while ((qk - buf_qk[1]).sqr_norm() / qk.sqr_norm() > epsilon); + } while (d_k_1 - d_k > epsilon); trace::exiting("registration::impl::icp_"); } Index: jardonnet/registration/icp.hh --- jardonnet/registration/icp.hh (revision 1978) +++ jardonnet/registration/icp.hh (working copy) @@ -101,65 +101,50 @@ std::cout << "k\t\te_k >=\td_k\tdqk" << std::endl; #endif - quat7<P::dim> buf_qk[4]; - float buf_dk[3]; + buffer<4,quat7<P::dim> > buf_qk; + buffer<3,float> buf_dk; - float err = 100; - //float err_bis; + float d_k = 10000; p_array<P> Ck(C); algebra::vec<P::dim,float> mu_C = center(C, c_length), mu_Xk; - buf_qk[0] = qk; + buf_qk.store(qk); qk.apply_on(C, Ck, c_length); + //buf_dk[0] = rms(C, map, c_length, buf_qk[1], qk); + 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; + buf_dk.store(d_k); //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; - + buf_qk.store(qk); //update qk - if (k > 3) - qk = update_qk(buf_qk, buf_dk); + 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); - // 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); + d_k = rms(C, map, c_length, buf_qk[1], qk); #ifndef NDEBUG + // e_k = d(closest(qk-1(P)), qk-1(P)) + float e_k = rms(C, map, c_length, buf_qk[1], buf_qk[1]); + //plot file - std::cout << k << '\t' << (e_k >= d_k ? ' ' : '-') << '\t' << e_k << '\t' << d_k << '\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 Index: jardonnet/registration/update_qk.hh --- jardonnet/registration/update_qk.hh (revision 1978) +++ jardonnet/registration/update_qk.hh (working copy) @@ -17,8 +17,8 @@ } - quat7<3> update_qk(quat7<3> qk[4], - float dk[3]) + quat7<3> update_qk(const quat7<3> qk[4], + const float dk[3]) { quat7<3> dqk_2 = qk[2] - qk[3]; quat7<3> dqk_1 = qk[1] - qk[2]; @@ -30,17 +30,18 @@ if (tetak < 10 and tetak_1 < 10) //10 degrees { float vk[3]; - float dk[3]; float coef[3]; vk[0] = 0; vk[1] = - dqk.sqr_norm(); vk[2] = - dqk_1.sqr_norm() + vk[1]; + // FIXME: check again. float a1 = (dk[2] + dk[0]) / (vk[2] - vk[0]); float b1 = dk[1] - ((dk[2] + dk[0]) / (vk[2] - vk[0])) * vk[1]; float v1 = math::abs(- b1 / a1); // > 0 + // FIXME: compute inline; polcoe(vk, dk, 2, coef); float a2 = coef[0]; float b2 = coef[1]; @@ -59,8 +60,6 @@ if (v1 > vmax and v2 > vmax) return qk[0] + (dqk / dqk.sqr_norm()) * vmax; - - std::cout << "echec" << std::endl; } return qk[0];