[Olena-patches] 1815: Sandbox: ICP : projection::memo.

https://svn.lrde.epita.fr/svn/oln/trunk/milena Index: ChangeLog from Ugo Jardonnet &lt;jardonnet(a)lrde.epita.fr&gt; Sandbox: ICP : projection::memo. * sandbox/jardonnet/test/icp.cc: Update. * sandbox/jardonnet/test/check: Test projection techniques. * sandbox/jardonnet/TODO: Update. * sandbox/jardonnet/registration/quat7.hh: Correction. * sandbox/jardonnet/registration/cloud.hh: Correction. * sandbox/jardonnet/registration/jacobi.hh: Add FIXME (nD). * sandbox/jardonnet/registration/icp.hh: Improvment. * sandbox/jardonnet/registration/projection.hh: Add a projection technique "memo" computing closest point only if needed. * sandbox/jardonnet/registration/tools.hh: Gather together tools to add to milena. mln/make/window3d.hh | 2 sandbox/jardonnet/TODO | 14 ++ sandbox/jardonnet/registration/cloud.hh | 4 sandbox/jardonnet/registration/icp.hh | 117 +++++++---------------- sandbox/jardonnet/registration/jacobi.hh | 2 sandbox/jardonnet/registration/projection.hh | 57 ++++++++++- sandbox/jardonnet/registration/quat7.hh | 50 ++------- sandbox/jardonnet/registration/tools.hh | 137 +++++++++++++++++++++++++++ sandbox/jardonnet/test/check | 13 ++ 9 files changed, 273 insertions(+), 123 deletions(-) Index: mln/make/window3d.hh --- mln/make/window3d.hh (revision 1814) +++ mln/make/window3d.hh (working copy) @@ -62,7 +62,7 @@ inline mln::window3d window3d(bool (&values)[M]) { - int h = unsigned(std::pow(float(M), 1 / 3)) / 2; + int h = unsigned(std::pow(float(M), float(1. / 3.))) / 2; mln_precondition((2 * h + 1) * (2 * h + 1) * (2 * h + 1) == M); mln::window3d tmp; unsigned i = 0; Index: sandbox/jardonnet/test/icp.cc Index: sandbox/jardonnet/test/check --- sandbox/jardonnet/test/check (revision 0) +++ sandbox/jardonnet/test/check (revision 0) @@ -0,0 +1,13 @@ +#!/bin/zsh + +touch log + +echo "### report : projection" >> log +echo "*** maps ***" >> log +time ./+icp_maps.exe +01.pbm +02.pbm >> log +echo "*** de_base ***" >> log +time ./+icp_base.exe +01.pbm +02.pbm >> log +echo "*** memo ***" >> log +time ./+icp_memo.exe +01.pbm +02.pbm >> log + +cat log Property changes on: sandbox/jardonnet/test/check ___________________________________________________________________ Name: svn:executable + * Index: sandbox/jardonnet/TODO --- sandbox/jardonnet/TODO (revision 1814) +++ sandbox/jardonnet/TODO (working copy) @@ -6,3 +6,17 @@ gaussian.cc:22: error: no match for 'operator==' in 'img == out' - - +adapt test for threshold (old thresholding) +- - + +- Enlever static dans projection::memo + +Note: ++01.bmp: 943 pts +30 itérations : 30*943 = 28290 calcul de ppp +Domaine +02.bmp: 777x480 = 362082 +nb de ppp calculé memo = 11749 + +map 1:40 +de_base 10.5 +memo 9.5 \ No newline at end of file Index: sandbox/jardonnet/registration/quat7.hh --- sandbox/jardonnet/registration/quat7.hh (revision 1814) +++ sandbox/jardonnet/registration/quat7.hh (working copy) @@ -15,37 +15,9 @@ # include <mln/trait/all.hh> +# include <mln/make/vec.hh> -// FIXME : move elsewhere -namespace mln -{ - namespace algebra - { - - template<unsigned n, unsigned m, typename T> - mat<m,n,T> - trans(const mat<n,m,T>& matrice) - { - mat<m,n,T> tmp; - for (unsigned i = 0; i < n; ++i) - for (unsigned j = 0; j < m; ++j) - tmp(j,i) = matrice(i,j); - return tmp; - } - - // trace - - template<unsigned n, typename T> inline - float tr(const mat<n,n,T>& m) - { - float f = 0.f; - for (unsigned i = 0; i < n; ++i) - f += m(i,i); - return f; - } - - } -} +# include "tools.hh" namespace mln { @@ -89,10 +61,15 @@ assert(input.npoints() == output.npoints()); assert(_qR.is_unit()); - //FIXME utiliser equivalent pour p_array - //std::transform(input.begin(), input.end(), - // output.begin(), - // *this); + for (size_t i = 0; i < input.npoints(); i++) + output.hook_()[i] = algebra::to_point<P>((*this)(input[i])); + } + + friend std::ostream& operator << (std::ostream& os, quat7& q) + { + std::cout << q._qR << std::endl; + std::cout << q._qT << std::endl; + return os; } }; @@ -121,8 +98,6 @@ // qR - //FIXME : use P::dim ? - std::cout << "loop" << std::endl; algebra::mat<P::dim,P::dim,float> Mk; for (unsigned i = 0; i < C.npoints(); ++i) { @@ -131,12 +106,11 @@ Mk += make::mat(Ci - mu_C) * trans(make::mat(Xki - mu_Xk)); } Mk /= C.npoints(); - std::cout << "loop end" << std::endl; const algebra::mat<P::dim,P::dim,float> Ak = Mk - trans(Mk); const float v[3] = {Ak(1,2), Ak(2,0), Ak(0,1)}; - const algebra::mat<3,1,float> D = make::mat<3,1,3,float>(v); // FIXME why <...> + const algebra::mat<3,1,float> D = make::mat<3,1,3,float>(v); algebra::mat<4,4,float> Qk; Index: sandbox/jardonnet/registration/cloud.hh --- sandbox/jardonnet/registration/cloud.hh (revision 1814) +++ sandbox/jardonnet/registration/cloud.hh (working copy) @@ -51,7 +51,7 @@ return f / a1.npoints(); } - } -} + } // end of namespace registration +} // end of namespace mln #endif // ndef CLOUD_HH Index: sandbox/jardonnet/registration/jacobi.hh --- sandbox/jardonnet/registration/jacobi.hh (revision 1814) +++ sandbox/jardonnet/registration/jacobi.hh (working copy) @@ -11,7 +11,7 @@ namespace mln { - + //FIXME: nD ? #define rotateJacobi(a,i,j,k,l) g=a(i,j);h=a(k,l);a(i,j)=g-s*(h+g*tau); \ a(k,l)=h+s*(g-h*tau); Index: sandbox/jardonnet/registration/icp.hh --- sandbox/jardonnet/registration/icp.hh (revision 1814) +++ sandbox/jardonnet/registration/icp.hh (working copy) @@ -35,7 +35,10 @@ # include <mln/algebra/quat.hh> # include <mln/algebra/vec.hh> -# include <mln/core/w_window.hh> +# include <mln/make/w_window.hh> +# include <mln/make/window3d.hh> + +# include "tools.hh" # include "cloud.hh" # include "quat7.hh" @@ -66,49 +69,47 @@ template <typename P, typename T1, typename T2> inline p_array<P> - icp_(const p_array<P>& C, - const p_array<P>&, - std::pair<T1,T2>& map) + icp_(p_array<P>& C, + const p_array<P>& X, + std::pair<T1,T2>&) { trace::entering("registration::impl::icp_"); unsigned int k; quat7<P::dim> old_qk, qk; - float err, err_bis; - p_array<P> Ck(C), Xk(C); + float err; + float err_bis; + p_array<P> Ck(C), Xk(C); //FIXME: Xk copy C algebra::vec<P::dim,float> mu_C = center(C), mu_Xk; - const float epsilon = 1e-3; + const float epsilon = 1e-2; - //step 1 + //// step 1 k = 0; - Ck = C; - do { - std::cout << "step2: projection" << std::endl; - //step 2 - err_bis = projection::fill_Xk(Ck, map, Xk); + //// step 2 + //err_bis = projection::fill_Xk(Ck, map, Xk); + //projection::de_base(Ck, X, Xk, err_bis); + projection::memo(Ck, X, Xk, err_bis); - std::cout << "step2.1: center" << std::endl; mu_Xk = center(Xk); - std::cout << "step3: Compute qk" << std::endl; - // step 3 + //// step 3 old_qk = qk; qk = match(C, mu_C, Xk, mu_Xk); - std::cout << "step4: apply qk" << std::endl; - // step 4 + //// step 4 qk.apply_on(C, Ck); // Ck+1 = qk(C) - // err = d(Ck+1,Xk) + //// err = d(Ck+1,Xk) err = rms(Ck, Xk); + std::cout << "error :" << err << std::endl; ++k; - std::cout << "error is " << err << std::endl; } while (k < 3 || (qk - old_qk).sqr_norm() > epsilon); + std::cout << "nb of iterations : " << k << std::endl; trace::exiting("registration::impl::icp_"); return Ck; } @@ -116,42 +117,6 @@ } // end of namespace mln::registration::impl - //FIXME: move? - namespace convert - { - template <typename T> - const image3d<T>& - to_image_3d(const image3d<T>& img) - { - return img; - } - - template <typename T> - image3d<T>& - to_image_3d(image3d<T>& img) - { - return img; - } - - template <typename T> - image3d<T> - to_image_3d(const image2d<T>& img) - { - point3d pmin(img.domain().pmin()[0], img.domain().pmin()[1], -10); - point3d pmax(img.domain().pmax()[0], img.domain().pmax()[1], 10); - image3d<T> img3d(box3d(pmin,pmax)); - - mln_piter(image3d<T>) p(img3d.domain()); - for_all(p) - { - if (p[2] == 0) - img3d(p) = exact(img)(point2d(p[0],p[1])); - } - return img3d; - } - } - - //Only for 2d and 3d image template <typename I, typename J> inline @@ -163,44 +128,38 @@ mln_precondition(exact(cloud_).has_data()); mln_precondition(exact(surface_).has_data()); - std::cout << "convert to image3d" << std::endl; //convert to image: time consuming typedef image3d<mln_value(I)> I3d; I3d cloud = convert::to_image_3d(exact(cloud_)); const I3d surface = convert::to_image_3d(exact(surface_)); - - std::cout << "chamfer" << std::endl; - - //FIXME: not a chamfer. etienne? + /* //create a pair (distance map, closest point) - // window<dpoint3d> win3d; - w_window<dpoint3d, float> chamfer; // = make::w_window(win3d, fun); - std::pair<mln_ch_value(I3d,float), mln_ch_value(I3d,mln_point(I3d)) > maps = - dt::chamfer(surface, chamfer); + bool w[27] = {true, true, true, true, true, true, true, true, true, + true, true, true, true, true, true, true, true, true, + true, true, true, true, true, true, true, true, true}; + window<mln_dpoint(I3d)> win3d = make::window3d(w); + fun::cham<point3d> fun; + w_window<mln_dpoint(I3d), float> chamfer = make::w_window(win3d, fun); + */ + std::pair<mln_ch_value(I3d,float), mln_ch_value(I3d,mln_point(I3d)) > maps;// = + //dt::chamfer(surface, chamfer); - std::cout << "Build p_array" << std::endl; - //build p_arrays. - p_array<mln_point(I3d)> c,x; - mln_piter(I3d) p1(cloud.domain()); - for_all(p1) - if (exact(cloud)(p1)) - c.append(p1); - - mln_piter(I3d) p2(surface.domain()); - for_all(p2) - if (exact(surface)(p2)) - x.append(p2); + //build p_arrays. + p_array<mln_point(I3d)> c = convert::to_p_array(cloud); + p_array<mln_point(I3d)> x = convert::to_p_array(surface); - std::cout << "Start ICP" << std::endl; p_array<mln_point(I3d)> res = impl::icp_(c, x, maps); //to 2d : projection (FIXME:if 3d) + //mln_concrete(I) output = convert::to_image2d(res)? mln_concrete(I) output(exact(cloud_).domain()); - for (unsigned i; i < res.npoints(); i++) + for (size_t i = 0; i < res.npoints(); i++) { point2d p(res[i][0], res[i][1]); + //FIXME: not necessary if output(res.bbox()) + //if (output.has(p)) output(p) = true; } Index: sandbox/jardonnet/registration/projection.hh --- sandbox/jardonnet/registration/projection.hh (revision 1814) +++ sandbox/jardonnet/registration/projection.hh (working copy) @@ -21,16 +21,18 @@ for (size_t i = 0; i < Ck.npoints(); ++i) { + //FIXME: bof + //if (map.second.has(Ck[i])) + //{ //x[i] := Ck[i] closest point in X Xk.hook_()[i] = map.second(Ck[i]); //err := Distance between Ck[i] and its closest point err += map.first(Ck[i]); + //} } return err /= Ck.npoints(); } - - template <typename P> void de_base(// input const p_array<P>& Ck, @@ -63,7 +65,58 @@ } err /= Ck.npoints(); } + + + template <typename P> + void memo(// input + const p_array<P>& Ck, + const p_array<P>& X, + // inout + p_array<P>& Xk, + float& err) + { + //FIXME: remove static + //FIXME: domain generated by X and C's domain? + static image3d<point3d> map(1000,1000,10); + static image3d<bool> mapset(1000,1000,10); + + assert(Ck.npoints() == Xk.npoints()); + + err = 0.f; + + for (size_t i = 0; i < Ck.npoints(); ++i) + { + float best_d; + if (mapset(Ck[i]) == false) + { + algebra::vec<P::dim,float> Cki = Ck[i]; + algebra::vec<P::dim,float> best_x = X[0]; + best_d = norm::l2(Cki - best_x); + for (size_t j = 1; j < X.npoints(); ++j) + { + algebra::vec<P::dim,float> Xj = X[j]; + float d = norm::l2(Cki - Xj); + if (d < best_d) + { + best_d = d; + best_x = Xj; } } + Xk.hook_()[i] = algebra::to_point<P>(best_x); + map(Ck[i]) = Xk[i]; + mapset(Ck[i]) = true; + } + else + { + Xk.hook_()[i] = map(Ck[i]); + } + err += best_d; + } + err /= Ck.npoints(); + } + + } // end of namespace projection + +} // end of namespace mln #endif // ndef PROJECTION_HH Index: sandbox/jardonnet/registration/tools.hh --- sandbox/jardonnet/registration/tools.hh (revision 0) +++ sandbox/jardonnet/registration/tools.hh (revision 0) @@ -0,0 +1,137 @@ +#ifndef REGISTRATION_TOOL_HH +# define REGISTRATION_TOOL_HH + +// temporary + +# include <mln/algebra/mat.hh> +# include <mln/core/p_array.hh> + +namespace mln +{ + + //FIXME: move? + namespace convert + { + + template <typename I> + inline + p_array<mln_point(I)> + to_p_array(const Image<I>& img_) + { + const I& img = exact(img_); + + p_array<mln_point(I)> a; + + mln_piter(I) p(img.domain()); + for_all(p) + if (img(p)) + a.append(p); + return a; + } + + //FIXME: to write + /* + template <typename A> + inline + image2d<mln_value(A)> + to_image2d(const A& a) + { + image2d<mln_value(A)> img(a.bbox()); + for (size_t i = 0; i < a.npoints(); i++) + { + point2d p(res[i][0], res[i][1]); + //FIXME: BOF + //if (output.has(p)) + output(p) = true; + } + } + */ + + template <typename T> + inline + const image3d<T>& + to_image_3d(const image3d<T>& img) + { + return img; + } + + template <typename T> + image3d<T>& + to_image_3d(image3d<T>& img) + { + return img; + } + + template <typename T> + inline + image3d<T> + to_image_3d(const image2d<T>& img) + { + point3d pmin(img.domain().pmin()[0], img.domain().pmin()[1], -1); + point3d pmax(img.domain().pmax()[0], img.domain().pmax()[1], 1); + image3d<T> img3d(box3d(pmin,pmax)); + + mln_piter(image3d<T>) p(img3d.domain()); + for_all(p) + { + if (p[2] == 0) + img3d(p) = exact(img)(point2d(p[0],p[1])); + } + return img3d; + } + + } // end of namespace convert + + + + namespace fun + { + //FIXME: temporary + template <typename C, typename T= float> + struct cham : public Function_p2v< cham<C,T> > + { + typedef T result; + //bad + T operator()(dpoints_fwd_piter<mln::dpoint_<mln::grid::cube, int> + { + C o = C::origin; + if (v < o) + return 1.; + else + return 0.; + } + }; + } // end of namespace fun + + + + namespace algebra + { + + template<unsigned n, unsigned m, typename T> + mat<m,n,T> + trans(const mat<n,m,T>& matrice) + { + mat<m,n,T> tmp; + for (unsigned i = 0; i < n; ++i) + for (unsigned j = 0; j < m; ++j) + tmp(j,i) = matrice(i,j); + return tmp; + } + + // trace + + template<unsigned n, typename T> inline + float tr(const mat<n,n,T>& m) + { + float f = 0.f; + for (unsigned i = 0; i < n; ++i) + f += m(i,i); + return f; + } + + } // end of namespace algebra + +} // end of namespace mln + +#endif // REGISTRATION_TOOLS_HH