https://svn.lrde.epita.fr/svn/oln/trunk/milena/sandbox Index: ChangeLog from Ugo Jardonnet <ugo.jardonnet@lrde.epita.fr> Sandbox: ICP: Update step by step rendering. Update example: * jardonnet/test/img/c5.pbm, * jardonnet/test/img/x5.pbm: Update . * jardonnet/test/Makefile: Add few warnings. * jardonnet/registration/jacobi.hh: Update: thresold e-6->e-12. * jardonnet/registration/power_it.hh: Fix. * jardonnet/registration/save.hh: Save Xk. * jardonnet/registration/tools.hh: Fix: signed -> unsigned. * jardonnet/registration/quat7.hh: Fix: signed -> unsigned. * jardonnet/registration/icp_ref.hh: Add saving before start. registration/icp_ref.hh | 10 ++++++---- registration/jacobi.hh | 4 +--- registration/power_it.hh | 29 +++++++++++++---------------- registration/quat7.hh | 4 ++-- registration/save.hh | 41 +++++++++++++++++++++++++++++++++++------ registration/tools.hh | 9 ++++++--- test/Makefile | 5 +++-- 7 files changed, 66 insertions(+), 36 deletions(-) Index: jardonnet/test/img/c5.pbm Cannot display: file marked as a binary type. svn:mime-type = application/octet-stream Index: jardonnet/test/img/x5.pbm Cannot display: file marked as a binary type. svn:mime-type = application/octet-stream Index: jardonnet/test/Makefile --- jardonnet/test/Makefile (revision 2037) +++ jardonnet/test/Makefile (working copy) @@ -20,7 +20,7 @@ g++ icp.o -I../../.. -O3 -DNDEBUG -o 'icp' icp_refD: icp_ref.cc ../registration/icp_ref.hh +depend - g++ icp_ref.cc -I../../.. -g -o 'icp_refD' + g++ -Wall -W icp_ref.cc -I../../.. -g -o 'icp_refD' icp_ref: icp_ref.cc ../registration/icp_ref.hh +depend g++ icp_ref.cc -I../../.. -O3 -DNDEBUG -o 'icp_ref' @@ -40,11 +40,12 @@ rm -f log.dat registred.pbm rm -f i_* rm -f tmp.ppm registred.ppm semantic.cache + rm +depend rm -f step_* .PHONY: check bench test icpD icp_refD icp icp_ref +depend: - g++ -M icp.cc $(FLAG) > +depend + g++ -MM icp.cc $(FLAG) > +depend include +depend \ No newline at end of file Index: jardonnet/registration/jacobi.hh --- jardonnet/registration/jacobi.hh (revision 2037) +++ jardonnet/registration/jacobi.hh (working copy) @@ -38,7 +38,7 @@ for (iq=ip+1;iq<4;iq++) sm += fabs(a(ip,iq)); } - if (sm < 1e-6) { //1e-12 + if (sm < 1e-12) { //1e-12 dd = d[0]; iq = 0; for (ip=1;ip<4;ip++) @@ -69,8 +69,6 @@ if (i > 4 && about_equal((float)(fabs(d[ip])+g), (float)fabs(d[ip])) && about_equal((float)(fabs(d[iq])+g), (float)fabs(d[iq]))) a(ip,iq)=0.0; - /* unusefull */ - else if (fabs(a(ip,iq)) > tresh) { h=d[iq]-d[ip]; //if ((float)(fabs(h)+g) == (float)fabs(h)) // unsafe ? Index: jardonnet/registration/power_it.hh --- jardonnet/registration/power_it.hh (revision 2037) +++ jardonnet/registration/power_it.hh (working copy) @@ -13,26 +13,23 @@ { template <uint n> - algebra::vec<n,float> power_it(algebra::mat<n,n,float> A) + algebra::vec<n,float> power_it(algebra::mat<n,n,float>& A) { - float normex = 1.; + algebra::vec<n,float> b0(literal::zero); + algebra::vec<n,float> bk(literal::zero); + for (unsigned i = 0; i < n; i++) + bk[i] = 0.1; + bk[0] = 1; + bk[1] = 1; - algebra::vec<n,float> x0(literal::zero); - algebra::vec<n,float> b(literal::zero); - - algebra::vec<n,float> x(literal::zero); - for (int i = 0; i < n; i++) - x[i] = 1.; - - //FIXME: infinit loop. - while (about_equal(norm::l2(x),norm::l2(x0))) + //FIXME: infinit loop + while (!about_equal(norm::l2(bk),norm::l2(b0))) { - x0 = x; - normex = norm::l2(x); - b = x / normex; - x = A * b; + b0 = bk; + bk = A * bk; + bk.normalize(); } - return x.normalize(); + return bk.normalize();; } } Index: jardonnet/registration/save.hh --- jardonnet/registration/save.hh (revision 2037) +++ jardonnet/registration/save.hh (working copy) @@ -33,10 +33,11 @@ namespace registration { - template<typename P> + template<typename P, typename M> void save_(const quat7<3>& qk, const p_array<P>& c, const p_array<P>& x, + const M& map, int every) { static int id = 0; @@ -49,14 +50,15 @@ p_array<P> ck(c); qk.apply_on(c, ck, c.npoints()); - const box_<P> working_box = enlarge(bigger(ck.bbox(),x.bbox()),100); + const box_<P> working_box = enlarge(bigger(ck.bbox(),x.bbox()),5); image2d<value::rgb8> out(convert::to_box2d(working_box), 1); level::fill(out, literal::white); //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 + + //Ck orientation algebra::mat<P::dim,P::dim,float> Mk(literal::zero); for (unsigned i = 0; i < ck.npoints(); ++i) { @@ -69,9 +71,28 @@ 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); + //build xk : project ck + p_array<P> xk; + for (unsigned i = 0; i < ck.npoints(); ++i) + xk.append(map(ck[i])); + + //plot mu_Xk + algebra::vec<P::dim,float> mu_Xk = center(xk, xk.npoints()); + draw::plot(out, point2d(mu_Xk[0], mu_Xk[1]), literal::blue); + + //Xk orientation + algebra::mat<P::dim,P::dim,float> Mxk(literal::zero); + for (unsigned i = 0; i < xk.npoints(); ++i) + { + algebra::vec<P::dim,float> Xki = xk[i]; + Mxk += make::mat(Xki - mu_Xk) * trans(make::mat(Xki - mu_Xk)); + } + Mxk /= c.npoints(); + algebra::vec<3,float> vxk = power_it(Mxk); + draw::line(out, point2d(mu_Xk[0], mu_Xk[1]), + point2d(mu_Xk[0]+vxk[0]*10, mu_Xk[1]+vxk[1]*10), + literal::red); + //ck in green for (unsigned i = 0; i < ck.npoints(); i++) @@ -89,6 +110,14 @@ out(p) = literal::black; } + //xk in blue + for (unsigned i = 0; i < xk.npoints(); i++) + { + point2d p(xk[i][0], xk[i][1]); + if (out.has(p)) + out(p) = literal::red; + } + //save std::stringstream oss; oss << "step_" << id++ << ".ppm"; Index: jardonnet/registration/tools.hh --- jardonnet/registration/tools.hh (revision 2037) +++ jardonnet/registration/tools.hh (working copy) @@ -43,7 +43,7 @@ void store(T e) { - for (int i = 0; i < n-1; i++) + for (unsigned i = 0; i < n-1; i++) buf[i+1] = buf[i]; buf[0] = e; @@ -70,7 +70,7 @@ template <typename P> struct closest_point { - typedef P input; + typedef algebra::vec<P::dim, float> input; typedef P result; closest_point(const p_array<P>& X, const box_<P>& box) @@ -137,8 +137,10 @@ const mln_result(F) //inline - operator () (const typename F::input& p) const + operator () (const typename F::input& p_) const { + point3d p = algebra::to_point<point3d>(p_); + mln_precondition(fun.domain().has(p)); //FIXME: What about domain? if (is_known(p)) @@ -205,6 +207,7 @@ for_all(p) if (img(p)) a.append(p); + return a; } Index: jardonnet/registration/quat7.hh --- jardonnet/registration/quat7.hh (revision 2037) +++ jardonnet/registration/quat7.hh (working copy) @@ -79,7 +79,7 @@ return os; } - float operator[](int i) + float operator[](unsigned i) { if (i < n) return _qT[i]; @@ -146,6 +146,7 @@ algebra::vec<P::dim,float> Xki = map(Ck[i]); Mk += make::mat(Ci - mu_C) * trans(make::mat(Xki - mu_Xk)); + // or Mk += make::mat(Ci) * trans(make::mat(Xki)) - make::mat(mu_C) * trans(make::mat(mu_Xk)) } Mk /= c_length; @@ -185,7 +186,6 @@ // qT const algebra::vec<P::dim,float> qT = mu_Xk - rotate(qR, mu_C); - return quat7<P::dim>(qR, qT); } Index: jardonnet/registration/icp_ref.hh --- jardonnet/registration/icp_ref.hh (revision 2037) +++ jardonnet/registration/icp_ref.hh (working copy) @@ -43,8 +43,7 @@ # include <mln/make/w_window3d.hh> # include <mln/value/rgb8.hh> -# include <mln/literal/colors.hh> -# include <mln/literal/black.hh> +# include <mln/literal/all.hh> # include <mln/level/fill.hh> # include <mln/io/ppm/save.hh> @@ -112,8 +111,12 @@ buf_qk.store(qk); + save_(qk,C,X,map,2); + qk.apply_on(C, Ck, c_length); + save_(qk,C,X,map,2); + unsigned int k = 0; do { @@ -135,7 +138,7 @@ e_k = rms(C, map, c_length, buf_qk[1], buf_qk[1]); #ifndef NDEBUG - save_(qk,C,X,2); + save_(qk,C,X,map,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' @@ -186,7 +189,6 @@ } #endif - //run icp for (int e = nb_it-1; e >= 0; e--) {