https://svn.lrde.epita.fr/svn/oln/trunk/milena/sandbox Index: ChangeLog from Ugo Jardonnet <jardonnet@lrde.epita.fr> Sandbox: ICP: final_qk. final_qk build a final quat7 qk considering the stddev * jardonnet/test/icp.cc: Refactoring. * jardonnet/test/plotscript: Auto update. * jardonnet/registration/tools.hh: Add final_qk 1 and 2. registration/quat7.hh | 1 registration/tools.hh | 102 +++++++++++++++++++++++++++++++++++++++++++++++++- test/icp.cc | 71 ++++++++-------------------------- test/plotscript | 4 - 4 files changed, 120 insertions(+), 58 deletions(-) Index: jardonnet/test/icp.cc --- jardonnet/test/icp.cc (revision 1940) +++ jardonnet/test/icp.cc (working copy) @@ -60,7 +60,7 @@ //FIXME: register img1 //init output image - //FIXME: Should be + //FIXME: Should be like //mln_concrete(I) output(res.bbox()) = convert::to_image<I>(res) ? qk.apply_on(c, c, c.npoints()); @@ -68,61 +68,23 @@ const box_<point2d> box2d(400,700); image2d<value::rgb8> output(box2d, 1); - std::vector<float> length(c.npoints()); - //mean + length - float 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(); -#ifndef NDEBUG - std::cout << "mean : " << mean << std::endl; -#endif + float stddev, mean; + registration::mean_stddev(c, map, mean, stddev); - //standar variation - float stdev = 0; - for (size_t i = 0; i < c.npoints(); i++) - stdev += (length[i] - mean) * (length[i] - mean); - stdev /= c.npoints(); - stdev = math::sqrt(stdev); #ifndef NDEBUG - std::cout << "stddev : " << stdev << std::endl; + std::cout << "mean : " << mean << std::endl; + std::cout << "stddev : " << stddev << std::endl; #endif - //final translate translate using point only separated less than 2*stdev - //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) - { - if (length[i] > 2 * stdev) - { - algebra::vec<3,float> xki = map(c[i]); - algebra::vec<3,float> ci = c[i]; - mu_C += ci; - - mu_Xk += xki; - nb_point++; - } - } - mu_C /= nb_point; - mu_Xk /= nb_point; + std::vector<float> length(c.npoints()); + for (size_t i = 0; i < c.npoints(); i++) + length[i] = norm::l2(algebra::vec<3,int> (c[i] - map(c[i]))); - // qT - const algebra::vec<3,float> qT = mu_Xk - mu_C; - //translate - for (size_t i = 0; i < c.npoints(); ++i) - { - algebra::vec<3,float> ci = c[i]; - ci -= qT; - c.hook_()[i] = algebra::to_point<point3d>(ci); - } + // final transform + quat7<3> fqk = registration::final_qk(c, map, 2 * stddev); + fqk.apply_on(c, c, c.npoints()); //to 2d : projection (FIXME:if 3d) for (size_t i = 0; i < c.npoints(); i++) @@ -131,12 +93,15 @@ point2d p(c[i][0], c[i][1]); if (output.has(p)) { - if (length[i] > 2 * stdev) - output(p) = value::rgb8(255,0,0); - else if (length[i] > stdev) + algebra::vec<3,float> xki = map(c[i]); + algebra::vec<3,float> ci = c[i]; + + if (length[i] > 2 * stddev) + output(p) = literal::red; + else if (length[i] > stddev) output(p) = value::rgb8(255,200,0); else - output(p) = value::rgb8(255,255,255); + output(p) = literal::white; } //Xk points point2d x(map(c[i])[0], map(c[i])[1]); Index: jardonnet/test/plotscript --- jardonnet/test/plotscript (revision 1940) +++ jardonnet/test/plotscript (working copy) @@ -1,11 +1,12 @@ -set title "Bench : q, e" - set xlabel "q" set ylabel "e" set zlabel "time" splot "log.dat" -save "plot.gp" -#pause(-1) \ No newline at end of file +set terminal png nocrop enhanced size 800,600 +set output "bench_qe.png" +replot +set output +set terminal x11 Index: jardonnet/registration/tools.hh --- jardonnet/registration/tools.hh (revision 1940) +++ jardonnet/registration/tools.hh (working copy) @@ -6,10 +6,111 @@ # include <mln/algebra/mat.hh> # include <mln/core/p_array.hh> +# include "quat7.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 + + //FIXME: groe length template <typename P> struct closest_point @@ -253,7 +354,6 @@ } // end of namespace convert - namespace algebra { Index: jardonnet/registration/quat7.hh --- jardonnet/registration/quat7.hh (revision 1940) +++ jardonnet/registration/quat7.hh (working copy) @@ -186,6 +186,7 @@ // qT const algebra::vec<P::dim,float> qT = mu_Xk - rotate(qR, mu_C); + return quat7<P::dim>(qR, qT); }