https://svn.lrde.epita.fr/svn/oln/trunk/milena Index: ChangeLog from Ugo Jardonnet <jardonnet@lrde.epita.fr> Sandbox: ICP: use of projection maps. * mln/binarization/thresholding.hh: Renamed as * mln/binarization/threshold.hh. * sandbox/jardonnet/test/icp.cc: Update: maps. * sandbox/jardonnet/registration/icp.hh: . * sandbox/jardonnet/registration/projection.hh: . mln/binarization/threshold.hh | 18 +++---- sandbox/jardonnet/registration/icp.hh | 63 +++++++++++---------------- sandbox/jardonnet/registration/jacobi.hh | 2 sandbox/jardonnet/registration/projection.hh | 25 +++++++++- sandbox/jardonnet/test/icp.cc | 4 - 5 files changed, 61 insertions(+), 51 deletions(-) Index: mln/binarization/threshold.hh --- mln/binarization/threshold.hh (revision 1811) +++ mln/binarization/threshold.hh (working copy) @@ -25,10 +25,10 @@ // reasons why the executable file might be covered by the GNU General // Public License. -#ifndef MLN_BINARIZATION_THRESHOLDING_HH -# define MLN_BINARIZATION_THRESHOLDING_HH +#ifndef MLN_BINARIZATION_THRESHOLD_HH +# define MLN_BINARIZATION_THRESHOLD_HH -/*! \file mln/binarization/thresholding.hh +/*! \file mln/binarization/threshold.hh * * \brief Threshold the contents of an image into another binary one. */ @@ -55,7 +55,7 @@ */ template <typename I> mln_concrete_ch_value(I, bool) - thresholding(const Image<I>& input, const mln_value(I) threshold); + threshold(const Image<I>& input, const mln_value(I) threshold); # ifndef MLN_INCLUDE_ONLY @@ -63,9 +63,9 @@ template <typename I> inline mln_concrete_ch_value(I, bool) - thresholding(const Image<I>& input, const mln_value(I) threshold) + threshold(const Image<I>& input, const mln_value(I) value) { - trace::entering("binarization::thresholding"); + trace::entering("binarization::threshold"); mln_precondition(exact(input).has_data()); mlc_is(mln_trait_value_nature(mln_value(I)), @@ -73,10 +73,10 @@ mln_concrete_ch_value(I, bool) output(exact(input).domain()); - fun::v2b::threshold< mln_value(I) > f(threshold); + fun::v2b::threshold< mln_value(I) > f(value); output = binarization::binarization(exact(input), f); - trace::exiting("binarization::thresholding"); + trace::exiting("binarization::threshold"); return output; } @@ -87,4 +87,4 @@ } // end of namespace mln -#endif // ! MLN_BINARIZATION_THRESHOLDING_HH +#endif // ! MLN_BINARIZATION_THRESHOLD_HH Index: sandbox/jardonnet/test/icp.cc --- sandbox/jardonnet/test/icp.cc (revision 1811) +++ sandbox/jardonnet/test/icp.cc (working copy) @@ -15,8 +15,6 @@ io::pbm::load(img1, argv[1]); io::pbm::load(img2, argv[2]); - image2d< bool > res(img1.domain()); - - io::pbm::save(registration::icp(img1,img2,res), "./+registred.pbm"); + io::pbm::save(registration::icp(img1,img2), "./+registred.pbm"); } Index: sandbox/jardonnet/registration/jacobi.hh --- sandbox/jardonnet/registration/jacobi.hh (revision 1811) +++ sandbox/jardonnet/registration/jacobi.hh (working copy) @@ -7,9 +7,11 @@ // from num. rec. in C + namespace mln { + #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 1811) +++ sandbox/jardonnet/registration/icp.hh (working copy) @@ -54,10 +54,9 @@ */ template <typename I, typename J> inline - I + mln_concrete(I) icp(const Image<I>& cloud, - const Image<J>& surface, - I& r); + const Image<J>& surface); # ifndef MLN_INCLUDE_ONLY @@ -66,23 +65,17 @@ template <typename P, typename T1, typename T2> inline - void + p_array<P> icp_(const p_array<P>& C, - const p_array<P>& X, - std::pair<T1,T2>&, - p_array<P>& Ck) + const p_array<P>&, + std::pair<T1,T2>& map) { trace::entering("registration::impl::icp_"); unsigned int k; quat7<P::dim> old_qk, qk; float err, err_bis; - - assert(Ck.npoints() == C.npoints()); - p_array<P> Xk(C); //FIXME:is it correct? - /// Ck.reserve(C.npoints()); - //Xk.reserve(C.npoints()); - //assert(C.npoints() != 0); + p_array<P> Ck(C), Xk(C); algebra::vec<P::dim,float> mu_C = center(C), mu_Xk; @@ -93,19 +86,19 @@ Ck = C; do { - std::cout << "step2" << std::endl; - //step 2 FIXME : etienne - projection::de_base(Ck, X, Xk, err_bis); + std::cout << "step2: projection" << std::endl; + //step 2 + err_bis = projection::fill_Xk(Ck, map, Xk); - std::cout << "step2.1 center" << std::endl; + std::cout << "step2.1: center" << std::endl; mu_Xk = center(Xk); - std::cout << "step3" << std::endl; + std::cout << "step3: Compute qk" << std::endl; // step 3 old_qk = qk; qk = match(C, mu_C, Xk, mu_Xk); - std::cout << "step4" << std::endl; + std::cout << "step4: apply qk" << std::endl; // step 4 qk.apply_on(C, Ck); // Ck+1 = qk(C) @@ -113,10 +106,11 @@ err = rms(Ck, Xk); ++k; - std::cout << err << std::endl; + std::cout << "error is " << err << std::endl; } while (k < 3 || (qk - old_qk).sqr_norm() > epsilon); trace::exiting("registration::impl::icp_"); + return Ck; } } // end of namespace mln::registration::impl @@ -161,10 +155,9 @@ //Only for 2d and 3d image template <typename I, typename J> inline - I + mln_concrete(I) icp(Image<I>& cloud_, - const Image<J>& surface_, - I& r) + const Image<J>& surface_) { trace::entering("registration::icp"); mln_precondition(exact(cloud_).has_data()); @@ -178,15 +171,13 @@ std::cout << "chamfer" << std::endl; - /* + //FIXME: not a chamfer. etienne? //create a pair (distance map, closest point) - w_window<mln_dpoint(I3d), float> chamfer; - */ - std::pair<mln_ch_value(I3d,float), mln_ch_value(I3d,mln_point(I3d)) > maps; - /* + // 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); - */ std::cout << "Build p_array" << std::endl; //build p_arrays. @@ -203,18 +194,18 @@ x.append(p2); std::cout << "Start ICP" << std::endl; + p_array<mln_point(I3d)> res = impl::icp_(c, x, maps); - p_array<mln_point(I3d)> res(c); - impl::icp_(c, x, maps, res); - - //to 2d - for (unsigned e; e < res.npoints(); e++) + //to 2d : projection (FIXME:if 3d) + mln_concrete(I) output(exact(cloud_).domain()); + for (unsigned i; i < res.npoints(); i++) { - point2d p(res[e][0], res[e][1]); - r(p) = true; + point2d p(res[i][0], res[i][1]); + output(p) = true; } trace::exiting("registration::icp"); + return output; } # endif // ! MLN_INCLUDE_ONLY Index: sandbox/jardonnet/registration/projection.hh --- sandbox/jardonnet/registration/projection.hh (revision 1811) +++ sandbox/jardonnet/registration/projection.hh (working copy) @@ -10,6 +10,27 @@ namespace projection { + template <typename P, typename T1, typename T2> + float fill_Xk(const p_array<P>& Ck, + std::pair<T1,T2>& map, + p_array<P>& Xk) + { + assert(Ck.npoints() == Xk.npoints()); + + float err = 0.f; + + for (size_t i = 0; i < Ck.npoints(); ++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, @@ -18,7 +39,7 @@ p_array<P>& Xk, float& err) { - // assert(Ck.npoints() == Xk.npoints()); + assert(Ck.npoints() == Xk.npoints()); err = 0.f; @@ -37,13 +58,11 @@ best_x = Xj; } } - if (i < Xk.npoints()) // FIXME:double hack Xk.hook_()[i] = algebra::to_point<P>(best_x); err += best_d; } err /= Ck.npoints(); } - } }