* doc/tutorial/Makefile.am: fix wrong include path. * mln/canvas/morpho/algebraic_union_find.hh: add missing MLN_INCLUDE_ONLY guards. * mln/core/clock_neighb.hh: add missing is_valid(). * mln/labeling/mean_values.hh, * mln/fun/v2v/rgb_to_hsl.hh: add missing includes. * mln/fun/x2x/rotation.hh: add more comments. * mln/io/dump/load.hh: check if the loaded image is valid. * mln/labeling/level.hh: Fix warnings. * mln/registration/all.hh: update comments. * mln/registration/icp2.hh: rename as... * mln/registration/icp.hh: ... this. * mln/registration/registration.hh: fix wrong call to registration_tests. * mln/util/graph.hh: add NDEBUG guards. * mln/value/rgb.hh: add missing forward declaration. --- milena/ChangeLog | 32 + milena/doc/tutorial/Makefile.am | 22 +- milena/mln/canvas/morpho/algebraic_union_find.hh | 12 + milena/mln/core/clock_neighb.hh | 18 +- milena/mln/fun/v2v/rgb_to_hsl.hh | 3 +- milena/mln/fun/x2x/rotation.hh | 3 + milena/mln/io/dump/load.hh | 2 + milena/mln/labeling/level.hh | 4 +- milena/mln/labeling/mean_values.hh | 9 +- milena/mln/registration/all.hh | 10 +- milena/mln/registration/icp.hh | 723 ++++++++++++++++++---- milena/mln/registration/icp2.hh | 693 --------------------- milena/mln/registration/registration.hh | 9 +- milena/mln/util/graph.hh | 2 + milena/mln/value/rgb.hh | 5 + 15 files changed, 692 insertions(+), 855 deletions(-) delete mode 100644 milena/mln/registration/icp2.hh diff --git a/milena/ChangeLog b/milena/ChangeLog index 1c446cd..6531f76 100644 --- a/milena/ChangeLog +++ b/milena/ChangeLog @@ -1,5 +1,37 @@ 2009-02-18 Guillaume Lazzara <z@lrde.epita.fr> + Various small fixes. + + * doc/tutorial/Makefile.am: fix wrong include path. + + * mln/canvas/morpho/algebraic_union_find.hh: add missing + MLN_INCLUDE_ONLY guards. + + * mln/core/clock_neighb.hh: add missing is_valid(). + + * mln/labeling/mean_values.hh, + * mln/fun/v2v/rgb_to_hsl.hh: add missing includes. + + * mln/fun/x2x/rotation.hh: add more comments. + + * mln/io/dump/load.hh: check if the loaded image is valid. + + * mln/labeling/level.hh: Fix warnings. + + * mln/registration/all.hh: update comments. + + * mln/registration/icp2.hh: rename as... + * mln/registration/icp.hh: ... this. + + * mln/registration/registration.hh: fix wrong call to + registration_tests. + + * mln/util/graph.hh: add NDEBUG guards. + + * mln/value/rgb.hh: add missing forward declaration. + +2009-02-18 Guillaume Lazzara <z@lrde.epita.fr> + Add support for 3D images in fun::x2v::bilinear. * mln/fun/x2v/bilinear.hh: Add a new specialization for 3d Images. diff --git a/milena/doc/tutorial/Makefile.am b/milena/doc/tutorial/Makefile.am index ceb4534..447dcbe 100644 --- a/milena/doc/tutorial/Makefile.am +++ b/milena/doc/tutorial/Makefile.am @@ -41,24 +41,24 @@ tutorial.tex \ tutorial.pdf .PHONY: regen-dist -regen-dist: $(srcdir)/headers.stamp +regen-dist: $(top_srcdir)/headers.stamp -$(srcdir)/examples/examples.mk: $(srcdir)/headers.stamp -$(srcdir)/figures/figures.mk: $(srcdir)/headers.stamp -$(srcdir)/outputs/outputs.mk: $(srcdir)/headers.stamp -$(srcdir)/samples/samples.mk: $(srcdir)/headers.stamp +$(top_srcdir)/milena/doc/tutorial/examples/examples.mk: $(top_srcdir)/headers.stamp +$(top_srcdir)/milena/doc/tutorial/figures/figures.mk: $(top_srcdir)/headers.stamp +$(top_srcdir)/milena/doc/tutorial/outputs/outputs.mk: $(top_srcdir)/headers.stamp +$(top_srcdir)/milena/doc/tutorial/samples/samples.mk: $(top_srcdir)/headers.stamp -EXTRA_DIST += $(srcdir)/headers.stamp -$(srcdir)/headers.stamp: $(srcdir)/generate_dist_files.sh +EXTRA_DIST += $(top_srcdir)/headers.stamp +$(top_srcdir)/headers.stamp: $(top_srcdir)/milena/doc/tutorial/generate_dist_files.sh @rm -f $@.tmp @touch $@.tmp cd $(srcdir) && ./generate_dist_files.sh @mv -f $@.tmp $@ -include $(srcdir)/examples/examples.mk -include $(srcdir)/figures/figures.mk -include $(srcdir)/outputs/outputs.mk -include $(srcdir)/samples/samples.mk +include $(top_srcdir)/milena/doc/tutorial/examples/examples.mk +include $(top_srcdir)/milena/doc/tutorial/figures/figures.mk +include $(top_srcdir)/milena/doc/tutorial/outputs/outputs.mk +include $(top_srcdir)/milena/doc/tutorial/samples/samples.mk EXTRA_DIST += \ tools/sample_utils.hh \ diff --git a/milena/mln/canvas/morpho/algebraic_union_find.hh b/milena/mln/canvas/morpho/algebraic_union_find.hh index 34a922c..a19ace8 100644 --- a/milena/mln/canvas/morpho/algebraic_union_find.hh +++ b/milena/mln/canvas/morpho/algebraic_union_find.hh @@ -51,6 +51,16 @@ namespace mln namespace morpho { + template <typename I, typename N, typename F> + inline + mln_concrete(I) + algebraic_union_find(const Image<I>& input, + const Neighborhood<N>& nbh, + F& f); + + +# ifndef MLN_INCLUDE_ONLY + namespace impl { @@ -393,6 +403,8 @@ namespace mln } +# endif // ! MLN_INCLUDE_ONLY + } // end of namespace mln::canvas::morpho } // end of namespace mln::canvas diff --git a/milena/mln/core/clock_neighb.hh b/milena/mln/core/clock_neighb.hh index 2c72506..1474978 100644 --- a/milena/mln/core/clock_neighb.hh +++ b/milena/mln/core/clock_neighb.hh @@ -1,5 +1,5 @@ -// Copyright (C) 2007, 2008 EPITA Research and Development Laboratory -// (LRDE) +// Copyright (C) 2007, 2008, 2009 EPITA Research and Development +// Laboratory (LRDE) // // This file is part of the Olena Library. This library is free // software; you can redistribute it and/or modify it under the terms @@ -105,6 +105,9 @@ namespace mln /// FIXME: not in constant time! mln::window<D> win() const; + /// Return whether this neighborhood is valid. + bool is_valid() const; + private: std::vector<D> vec_; }; @@ -145,6 +148,17 @@ namespace mln result.insert(vec_[i]); return result; } + + template <typename D> + inline + bool + clock_neighb<D>::is_valid() const + { + //FIXME: correct? + return true; + } + + # endif // ! MLN_INCLUDE_ONLY } // end of namespace mln diff --git a/milena/mln/fun/v2v/rgb_to_hsl.hh b/milena/mln/fun/v2v/rgb_to_hsl.hh index 73a68f2..39433f2 100644 --- a/milena/mln/fun/v2v/rgb_to_hsl.hh +++ b/milena/mln/fun/v2v/rgb_to_hsl.hh @@ -36,6 +36,8 @@ # include <mln/trait/value_.hh> +# include <mln/value/rgb8.hh> +# include <mln/value/rgb16.hh> namespace mln { @@ -43,7 +45,6 @@ namespace mln // Forward declaration namespace value { - template <unsigned n> struct rgb; template <typename H, typename S, typename L> class hsl_; typedef hsl_<float, float, float> hsl_f; } diff --git a/milena/mln/fun/x2x/rotation.hh b/milena/mln/fun/x2x/rotation.hh index 9628c26..9addb50 100644 --- a/milena/mln/fun/x2x/rotation.hh +++ b/milena/mln/fun/x2x/rotation.hh @@ -35,6 +35,9 @@ /// /// \todo store the quaternion instead of (axis, alpha) /// => better precision while composing two rotation matrices. +/// +/// Conversion from Quaternion to (angle,axis), Source: +/// http://jeux.developpez.com/faq/matquat/?page=quaternions#Q56 # include <cmath> # include <mln/core/concept/function.hh> diff --git a/milena/mln/io/dump/load.hh b/milena/mln/io/dump/load.hh index 7bbce9e..21d6534 100644 --- a/milena/mln/io/dump/load.hh +++ b/milena/mln/io/dump/load.hh @@ -129,6 +129,8 @@ namespace mln internal::load_header(ima, file); internal::load_data(ima, file); + mln_postcondition(exact(ima).is_valid()); + trace::exiting("mln::io::dump::load"); } diff --git a/milena/mln/labeling/level.hh b/milena/mln/labeling/level.hh index 050b2d0..11b3a06 100644 --- a/milena/mln/labeling/level.hh +++ b/milena/mln/labeling/level.hh @@ -121,9 +121,9 @@ namespace mln bool handles_(unsigned p) const { return input.element(p) == val; } bool equiv_(unsigned n, unsigned) const { return input.element(n) == val; } bool labels_(unsigned) const { return true; } - void do_no_union_(unsigned n, unsigned p) {} + void do_no_union_(unsigned, unsigned) {} void init_attr_(unsigned) {} - void merge_attr_(unsigned r, unsigned p) {} + void merge_attr_(unsigned, unsigned) {} // end of Requirements. diff --git a/milena/mln/labeling/mean_values.hh b/milena/mln/labeling/mean_values.hh index d7c131a..2ea06e4 100644 --- a/milena/mln/labeling/mean_values.hh +++ b/milena/mln/labeling/mean_values.hh @@ -29,6 +29,13 @@ # define MLN_LABELING_MEAN_VALUES_HH # include <mln/core/concept/image.hh> +# include <mln/core/alias/vec3d.hh> + +# include <mln/accu/mean.hh> + +# include <mln/labeling/compute.hh> + +# include <mln/literal/colors.hh> /// \file mln/labeling/mean_values.hh /// @@ -116,7 +123,7 @@ namespace mln util::array<mln_value(I)> m; convert::from_to(m_3f, m); - m[0] = literal::white; //FIXME: handle label 0 correctly. + m[0] = literal::yellow; //FIXME: handle label 0 correctly. mln_concrete(I) output = level::transform(lbl, convert::to< fun::i2v::array<mln_value(I)> >(m)); diff --git a/milena/mln/registration/all.hh b/milena/mln/registration/all.hh index dd29f26..bb4e18e 100644 --- a/milena/mln/registration/all.hh +++ b/milena/mln/registration/all.hh @@ -1,4 +1,5 @@ -// Copyright (C) 2008 EPITA Research and Development Laboratory +// Copyright (C) 2008, 2009 EPITA Research and Development Laboratory +// (LRDE) // // This file is part of the Olena Library. This library is free // software; you can redistribute it and/or modify it under the terms @@ -28,10 +29,9 @@ #ifndef MLN_REGISTRATION_ALL_HH # define MLN_REGISTRATION_ALL_HH -/*! \file mln/registration/all.hh - * - * \brief File that includes all "point-wise" expression tools. - */ +/// \file mln/registration/all.hh +/// +/// File that includes all "point-wise" expression tools. namespace mln diff --git a/milena/mln/registration/icp.hh b/milena/mln/registration/icp.hh index b116b0e..de763ea 100644 --- a/milena/mln/registration/icp.hh +++ b/milena/mln/registration/icp.hh @@ -1,4 +1,4 @@ -// Copyright (C) 2008 EPITA Research and Development Laboratory +// Copyright (C) 2009 EPITA Research and Development Laboratory (LRDE) // // This file is part of the Olena Library. This library is free // software; you can redistribute it and/or modify it under the terms @@ -28,217 +28,666 @@ #ifndef MLN_REGISTRATION_ICP_HH # define MLN_REGISTRATION_ICP_HH -/*! \file mln/registration/icp.hh - * - * \brief Register an image over an another using the ICP algorithm. - */ +/// \file mln/registration/icp.hh +/// +/// Register an image over an another using the ICP algorithm. +/// +/// \todo encode distances on 12 bits. +# include <cmath> +# include <algorithm> + +# include <mln/core/alias/vec3d.hh> +# include <mln/math/jacobi.hh> # include <mln/fun/x2x/all.hh> # include <mln/fun/x2v/all.hh> -# include <mln/registration/get_rtransf.hh> -# include <mln/registration/internal/rms.hh> +# include <mln/convert/to.hh> +# include <mln/accu/compute.hh> +# include <mln/accu/center.hh> +# include <mln/accu/rms.hh> +# include <mln/trait/image_from_grid.hh> +# include <mln/set/compute.hh> + +//Should be removed when closest_point functors are moved. +# include <mln/core/alias/neighb3d.hh> +# include <mln/transform/internal/closest_point_functor.hh> +# include <mln/canvas/distance_geodesic.hh> +# include <mln/pw/all.hh> + +# include <mln/io/ppm/save.hh> +# include <mln/io/pbm/save.hh> +# include <mln/debug/colorize.hh> + +# include <mln/literal/black.hh> +# include <mln/literal/white.hh> +# include <mln/literal/colors.hh> + +# include <mln/core/image/slice_image.hh> + +#include <mln/core/image/tr_image.hh> +#include <mln/core/image/extension_fun.hh> + +#include <mln/accu/histo.hh> +#include <mln/accu/sum.hh> +#include <mln/debug/histo.hh> + +# include <mln/util/timer.hh> namespace mln { - namespace util + + namespace registration { - // FIXME: Remove use of this class - template <unsigned int n, typename T> - struct buffer + //FIXME: used for debug purpose. Should be removed later. + + using namespace fun::x2x; + + /*! Register point in \p c using a function of closest points + * \p closest_point. + * + * \param[in] P_ The cloud of points. + * \param[in] X the reference surface. + * \param[in] closest_point The function of closest points. + * \param[out] qk The rigid transformation obtained. + * + * \return the rigid transformation which may be use later to create + * a registered image. + * + * WARNING: the function \p closest_point *MUST* take float/double + * vector as arguments. Otherwise the resulting transformation may be + * wrong due to the truncation of the vector coordinate values. + * + * \pre \p P_ and \p X must not be empty. + * + * Reference article: "A Method for Registration of 3-D Shapes", Paul J. + * Besl and Neil D. McKay, IEEE, 2, February 1992. + * + */ + template <typename P, typename F> + std::pair<algebra::quat,mln_vec(P)> + icp(const p_array<P>& P_, + const p_array<P>& X, + const F& closest_point, + const algebra::quat& initial_rot, + const mln_vec(P)& initial_translation); + + + template <typename P, typename F> + composed< translation<P::dim,float>,rotation<P::dim,float> > + icp(const p_array<P>& P_, + const p_array<P>& X, + const F& closest_point); + + +# ifndef MLN_INCLUDE_ONLY + + + /// Closest point functor based on map distance. + template <typename P> + class closest_point_with_map { - buffer() - : setted(0) + typedef mln_image_from_grid(mln_grid(P), P) I; + typedef mln_ch_value(I, P) cp_ima_t; + typedef mln_ch_value(I,value::int_u16) dmap_t; + + public: + + closest_point_with_map(const p_array<P>& X) { + box3d box = geom::bbox(X); + box.enlarge(1, box.nrows()); + box.enlarge(2, box.ncols()); + + mln_postcondition(box.is_valid()); + + std::cout << "Map image defined on " << box << std::endl; + + init(X, box); } - void store(T e) + closest_point_with_map(const p_array<P>& X, const box<P>& box) { - for (unsigned i = 0; i < n-1; i++) - buf[i+1] = buf[i]; - buf[0] = e; - - setted++; + init(X, box); } - T& operator[](unsigned int i) + void init(const p_array<P>& X, const box<P>& box) { - assert(i < n && i < setted); - return buf[i]; + typedef mln_ch_value(I, bool) model_t; + model_t model(box); + data::fill(model, false); + data::fill((model | X).rw(), true); + + transform::internal::closest_point_functor<model_t> f; + util::timer t; + t.start(); + dmap_X_ = canvas::distance_geodesic(model, c6(), + mln_max(value::int_u16), + f); + std::cout << "canvas::distance_geodesic - " << t << "s" << std::endl; + + initialize(this->cp_ima_, f.cp_ima); + this->cp_ima_ = f.cp_ima; + + mln_postcondition(cp_ima_.is_valid()); + mln_postcondition(cp_ima_.domain().is_valid()); + std::cout << "pmin = " << cp_ima_.domain().pmin() << std::endl;; + std::cout << "pmax = " << cp_ima_.domain().pmax() << std::endl;; + +#ifndef NDEBUG +// mln_ch_value(I, bool) debug2(box); +// data::fill(debug2, false); +// mln_ch_value(I, value::rgb8) debug(box); +// mln_piter(p_array<P>) p(X); +// for_all(p) +// { +// debug(p) = debug::internal::random_color(value::rgb8()); +// debug2(p) = true; +// } +// io::pbm::save(slice(debug2,0), "debug2-a.ppm"); +// +// mln_piter(I) pi(cp_ima_.domain()); +// for_all(pi) +// { +// debug(pi) = debug(cp_ima_(pi)); +// debug2(pi) = debug2(cp_ima_(pi)); +// } +// +// io::pbm::save(slice(debug2,0), "debug2-b.ppm"); +// io::ppm::save(slice(debug,0), "debug.ppm"); +// std::cout << "map saved" << std::endl; +#endif } - const T * get_array() + mln_site(I) operator()(const mln_site(I)& p) const { - return buf; + return cp_ima_(p); } + // Distance map + dmap_t dmap_X_; + // Closest point image. + cp_ima_t cp_ima_; private: - T buf[n]; - unsigned int setted; }; - } // end of namespace mln::util - namespace registration - { + /// Closest point functor based on map distance. + template <typename P> + class closest_point_basic + { + typedef mln_image_from_grid(mln_grid(P), P) I; + typedef p_array<P> X_t; - using namespace fun::x2x; + public: - /*! Register point in \p c using a map of closest points \p map - * - * \param[in] c The cloud of points. - * \param[in] map The map of closest points. - * \param[in] epsilon ICP stops if sqr_norm(qk - qk-1) / - * sqr_norm(qk) > epsilon - * \param[out] qk The rigid transformation obtained. - * - * \pre \p ima has to be initialized. - */ - template <typename P, typename M> - inline - composed<rotation<P::dim, float>, translation<P::dim, float> > - icp(const p_array<P>& c, const M& map, - const float epsilon = 1e-3); + closest_point_basic(const p_array<P>& X) + : X_(X) + { + } + mln_site(I) operator()(const vec3d_f& v) const + { + vec3d_f best_x = convert::to<vec3d_f>(X_[0].to_vec()); + + float best_d = norm::l2_distance(v, best_x); + mln_piter(X_t) X_i(X_); + for_all(X_i) + { + float d = norm::l2_distance(v, convert::to<vec3d_f>(X_i)); + if (d < best_d) + { + best_d = d; + best_x = X_i.to_vec(); + } + } + return best_x; + } - /*! Register point in \p c using a map of closest points \p map - * - * \param[in] c The cloud of points. - * \param[in] c_length points from cloud to use. - * \param[in] map The map of closest points. - * \param[in] qk The initial rigid transformation applied. - * \param[in] epsilon ICP stops if sqr_norm(qk - qk-1) / - * sqr_norm(qk) > epsilon - * \param[out] qk The rigid transformation obtained. - * - * \pre \p ima has to be initialized. - */ - template <typename P, typename M, typename T> - inline + private: + const p_array<P>& X_; + }; + + + template <typename P> void - icp_subset(const p_array<P>& c, - const unsigned int c_length, - const M& map, - T& qk, - float epsilon = 1e-3); + draw_last_run(const box3d& box, const p_array<P>& kept, + const p_array<P>& removed, const p_array<P>& X, + const algebra::quat& qR, const vec3d_f qT) + { + typedef image3d<value::rgb8> result_t; + result_t result(box); + typedef extension_fun<result_t,pw::cst_<mln_value(result_t)> > ext_result_t; + ext_result_t ext_result(result, pw::cst(value::rgb8(0,0,0))); + data::fill(ext_result, literal::black); + data::fill((ext_result | X).rw(), literal::white); -# ifndef MLN_INCLUDE_ONLY + mln_piter(p_array<P>) p(kept); + for_all(p) + ext_result(qR.rotate(p.to_vec()) + qT) = literal::green; + + mln_piter(p_array<P>) p2(removed); + for_all(p2) + ext_result(qR.rotate(p2.to_vec()) + qT) = literal::red; - namespace impl + io::ppm::save(slice(ext_result,0), "registered-2.ppm"); + } + + + + template <typename P, typename F> + float compute_standard_deviation(const p_array<P>& P_, + const std::pair<algebra::quat,mln_vec(P)>& pair, + const F& closest_point) { + accu::rms<vec3d_f,float> e_k_accu; - // FIXME: Move elsewhere - template <typename P> - algebra::vec<P::dim,float> - center(const p_array<P>& a) + // Standard Deviation + float sd; + mln_piter(p_array<P>) p(P_); + for_all(p) { - algebra::vec<P::dim,float> c(literal::zero); - for (unsigned i = 0; i < a.nsites(); ++i) - c += convert::to< algebra::vec<P::dim,float> > (a[i]); + vec3d_f Pk_i = pair.first.rotate(p.to_vec()) + pair.second; + vec3d_f Yk_i = closest_point(Pk_i).to_vec(); + // yk_i - pk_i + e_k_accu.take(Yk_i - Pk_i); + } + + float d = e_k_accu.to_result(); + sd = std::sqrt(e_k_accu.hook_value_() / P_.nsites() - d * d); + return sd; + } + - return c / a.nsites(); + template <typename P, typename F> + void + remove_too_far_sites_debug(image3d<value::rgb8>& out, const p_array<P>& P_, + const F& closest_point, + const std::pair<algebra::quat,mln_vec(P)>& pair, + const p_array<P>& X, + unsigned r, int d_min, int d_max, unsigned prefix) + { + unsigned removed = 0; + accu::histo<value::int_u8> h; + mln_piter(p_array<P>) p(P_); + data::fill(out, literal::black); + data::fill((out | X).rw(), literal::white); + + for_all(p) + { + vec3d_f Pk_i = pair.first.rotate(p.to_vec()) + pair.second; + vec3d_f Yk_i = closest_point(Pk_i).to_vec(); + + int d_i = closest_point.dmap_X_(Pk_i); + if (d_i >= d_min && d_i <= d_max) + out(Pk_i) = literal::green; + else + { + ++removed; + out(Pk_i) = literal::red; + } } - // FIXME: Make use of level::apply - template <typename P, typename F> - void apply(F& f, - const p_array<P>& a, - p_array<P>& b) +#ifndef NDEBUG + std::ostringstream ss1; + ss1 << "histo_" << prefix << r << ".dat"; + std::cout << h << std::endl; + + std::ostringstream ss2; + ss2 << "out_" << prefix << r << ".ppm"; + io::ppm::save(mln::slice(out,0), ss2.str()); +#endif + std::cout << "Points removed with the whole set and current d_min/d_max: " << removed << std::endl; + + } + + + template <typename P, typename F> + void + compute_distance_criteria(const p_array<P>& P_, + const F& closest_point, + const std::pair<algebra::quat,mln_vec(P)>& pair, + unsigned r, int& d_min, int& d_max) + { + mln_piter(p_array<P>) p(P_); + accu::histo<value::int_u8> h; + + float sd; { - for (unsigned i = 0; i < a.nsites(); i++) - b[i] = f(convert::to< algebra::vec<P::dim,float> >(a[i])); + accu::sum<float> s, s2; + for_all(p) + { + vec3d_f Pk_i = pair.first.rotate(p.to_vec()) + pair.second; + unsigned d_i = closest_point.dmap_X_(Pk_i); + h.take(d_i); + s.take(d_i); + s2.take(d_i * d_i); + } + float mean = s / P_.nsites(); + sd = std::sqrt(s2 / P_.nsites() - mean * mean); + d_min = int(mean - sd); + d_max = int(mean + sd); } - template <typename P, typename M, typename T> - inline - void - icp_(const p_array<P>& c_, - const unsigned c_length, - const M& map, - T& qk, - const float epsilon = 1e-3) + std::cout << "Standard deviation = " << sd << std::endl; + std::ostringstream ss1; + ss1 << "histo_" << r << ".dat"; + std::cout << h << std::endl; + std::cout << "d thresholds = " << d_min << ' ' << d_max << std::endl; + } + + template <typename P, typename F> + p_array<P> + remove_too_far_sites(image3d<value::rgb8>& out, const p_array<P>& P_, + const F& closest_point, + const std::pair<algebra::quat,mln_vec(P)>& pair, + const p_array<P>& X, p_array<P>& removed_set, + unsigned r, int d_min, int d_max, + const std::string& method) + { + p_array<P> tmp; + unsigned removed = 0; + +# ifndef NDEBUG + data::fill(out, literal::black); + data::fill((out | X).rw(), literal::white); +# endif // ! NDEBUG + + mln_piter(p_array<P>) p(P_); + for_all(p) { - trace::entering("registration::impl::icp_"); + vec3d_f Pk_i = pair.first.rotate(p.to_vec()) + pair.second; + vec3d_f Yk_i = closest_point(Pk_i).to_vec(); + + int d_i = closest_point.dmap_X_(Pk_i); + if (d_i >= d_min && d_i <= d_max) + { + tmp.append(p); + out(Pk_i) = literal::green; + } + else + { + ++removed; + removed_set.append(p); + out(Pk_i) = literal::red; + } + } + - util::buffer<4,T> buf_qk; - util::buffer<3,float> buf_dk; +# ifndef NDEBUG + std::ostringstream ss2; + ss2 << method << "_" << r << "_removed_sites" << ".ppm"; + io::ppm::save(mln::slice(out,0), ss2.str()); + + std::cout << "Points removed: " << removed << std::endl; +# endif // ! NDEBUG + // They are used for debug purpose only. + // When NDEBUG is set, they are unused. + (void) X; + (void) r; + (void) method; + + return tmp; + } - float d_k = 10000; + template <typename P> + void + display_sites_used_in_icp(image3d<value::rgb8>& out, const p_array<P>& P_sub, + const p_array<P>& P_, const p_array<P>& X, + unsigned r, const std::string& prefix, + const std::pair<algebra::quat,mln_vec(P)>& pair, + const std::string& period, const value::rgb8& c) + { +# ifndef NDEBUG + data::fill(out, literal::black); + data::fill((out | X).rw(), literal::white); +# endif // !NDEBUG - //work on a reduced version of c_ - p_array<P> c = c_; - //FIXME: Use c_ and specify c_length to function using c_ - c.hook_std_vector_().resize(c_length); + mln_piter(p_array<P>) p1(P_); + for_all(p1) + { + vec3d_f Pk_i = pair.first.rotate(p1.to_vec()) + pair.second; + out(Pk_i) = literal::red; + } - //FIXME: avoid use of ck - p_array<P> ck(c); - algebra::vec<P::dim,float> mu_c = center(c); + mln_piter(p_array<P>) p2(P_sub); + for_all(p2) + { + vec3d_f Pk_i = pair.first.rotate(p2.to_vec()) + pair.second; + out(Pk_i) = c; + } - buf_qk.store(qk); +# ifndef NDEBUG + std::ostringstream ss; + ss << prefix << "_" << r << "_" << period << ".ppm"; - apply(qk, c, ck); + io::ppm::save(slice(out,0), ss.str()); +# endif // ! NDEBUG + } - do { - std::cout << "*------" << std::endl; - buf_dk.store(d_k); - //compute qk - qk = get_rtransf(c, mu_c, ck, map); - buf_qk.store(qk); + template <typename P, typename F> + inline + float + compute_d_k(const p_array<P>& P_, + const F& closest_point, + const algebra::quat& qR, + const algebra::quat& qR_old, + const vec3d_f& qT, + const vec3d_f& qT_old) + { + accu::rms<vec3d_f, float> accu; + mln_piter(p_array<P>) p(P_); + for_all(p) + { + // yk_i - pk+1_i + vec3d_f Pk_i = qR_old.rotate(p.to_vec()) + qT_old; + vec3d_f Pk_1_i = qR.rotate(p.to_vec()) + qT; + accu.take(closest_point(Pk_i).to_vec() - Pk_1_i); + } + return accu.to_result(); + } - //Ck+1 = qk(c) - apply(qk, c, ck); - // d_k = d(Yk, Pk+1) - // = d(closest(qk-1(P)), qk(P)) - d_k = internal::rms(c, map, buf_qk[1], qk); + /// FIXME: work only for 3d images. + template <typename P, typename F> + algebra::quat + get_rot(const p_array<P>& P_, + const vec3d_f& mu_P, + const vec3d_f& mu_Yk, + const F& closest_point, + const algebra::quat& qR, + const vec3d_f& qT) + { + /// Spx: cross-covariance matrix. + algebra::mat<3u,3u,float> Spx; + mln_piter(p_array<P>) p(P_); - //FIXME: Add matrix norm - //} while ((qk - buf_qk[1]).sqr_norm() / qk.sqr_norm() > epsilon); + // FIXME: could we use an accu? + for_all(p) + { + vec3d_f P_i = p.to_vec(); + vec3d_f Pk_i = qR.rotate(p.to_vec()) + qT; + vec3d_f Yk_i = closest_point(Pk_i).to_vec(); + Spx += make::mat(P_i - mu_P) * trans(make::mat(Yk_i - mu_Yk)); + } + Spx /= P_.nsites(); - std::cout << d_k << std::endl; + vec3d_f A; + A[0] = Spx(1,2) - Spx(2,1); + A[1] = Spx(2,0) - Spx(0,2); + A[2] = Spx(0,1) - Spx(1,0); - } while (d_k > 9.2 + epsilon); + algebra::mat<4u,4u,float> Qk; + float t = tr(Spx); - trace::exiting("registration::impl::icp_"); + Qk(0,0) = t; + for (int i = 1; i < 4; ++i) + { + Qk(i,0) = A[i - 1]; + Qk(0,i) = A[i - 1]; + for (int j = 1; j < 4; ++j) + if (i == j) + Qk(i,j) = 2 * Spx(i - 1,i - 1) - t; } - } // end of namespace mln::registration::impl + Qk(1,2) = Spx(0,1) + Spx(1,0); + Qk(2,1) = Spx(0,1) + Spx(1,0); + + Qk(1,3) = Spx(0,2) + Spx(2,0); + Qk(3,1) = Spx(0,2) + Spx(2,0); + + Qk(2,3) = Spx(1,2) + Spx(2,1); + Qk(3,2) = Spx(1,2) + Spx(2,1); + + return math::jacobi(Qk); + } + - template <typename P, typename M> + // Compute mu_Yk, mass center of Yk. + template <typename P, typename F> inline - composed<rotation<P::dim, float>, translation<P::dim, float> > - icp(const p_array<P>& c, const M& map, - const float epsilon = 1e-3) + vec3d_f + get_mu_yk(const p_array<P>& P_, + const F& closest_point, + const algebra::quat& qR, + const vec3d_f& qT, + float& e_k) { - composed<rotation<P::dim, float>, translation<P::dim, float> > qk; + accu::rms<vec3d_f,float> e_k_accu; + accu::center<P,vec3d_f> mu_yk; - std::cout << " icp " << std::endl; + mln_piter(p_array<P>) p(P_); + for_all(p) + { + // yk_i - pk_i + vec3d_f Pk_i = qR.rotate(p.to_vec()) + qT; + vec3d_f Yk_i = closest_point(Pk_i).to_vec(); + mu_yk.take(Yk_i); + e_k_accu.take(Yk_i - Pk_i); + } - impl::icp_(c, c.nsites(), map, qk, epsilon); - return qk; + e_k = e_k_accu.to_result(); + return mu_yk.to_result(); } - template <typename P, typename M, typename T> + + + /// Base version of the ICP algorithm. It is called in other variants. + template <typename P, typename F> inline - void - icp_subset(const p_array<P>& c, - const unsigned int c_length, - const M& map, - T& qk, - float epsilon = 1e-3) + std::pair<algebra::quat,mln_vec(P)> + icp(const p_array<P>& P_, + const p_array<P>& X, + const F& closest_point, + const algebra::quat& initial_rot, + const mln_vec(P)& initial_translation) { - impl::icp_(c, c_length, map, qk, epsilon); + trace::entering("registration::icp"); + + (void) X; + mln_precondition(P::dim == 3); + mln_precondition(!P_.is_empty()); + mln_precondition(!X.is_empty()); + mln_precondition(!initial_rot.is_null()); + + typedef p_array<P> cloud_t; + + vec3d_f mu_P = set::compute(accu::center<P,vec3d_f>(), P_); + + vec3d_f qT_old, qT = initial_translation; + algebra::quat qR_old, qR = initial_rot; + float e_k, e_k_old = mln_max(float); + float d_k, d_k_old = mln_max(float); + unsigned k = 0; + +# ifndef NDEBUG + box3d box = geom::bbox(X); + //FIXME: too large? + box.enlarge(1, box.nrows() / 2); + box.enlarge(2, box.ncols() / 2); + image3d<value::rgb8> debug(box); + data::fill(debug, literal::black); + data::fill((debug | X).rw(), literal::white); +# endif + + do + { + qT_old = qT; + qR_old = qR; + + /// Compute transformation + /// + // mu_Yk - Pk's mass center. + // + Compute error ek = d(Pk,Yk) + vec3d_f mu_Yk = get_mu_yk(P_, closest_point, qR_old, qT_old, e_k); + + // quaternion qR - rotation + qR = get_rot(P_, mu_P, mu_Yk, closest_point, qR_old, qT_old); + vec3d_f tmp = qR.v(); + + // vector qT - translation + qT = mu_Yk - qR.rotate(mu_P); + /// + /// End of "compute transformation" + + // Distance dk = d(Pk+1, Yk) + d_k = compute_d_k(P_, closest_point, qR, qR_old, qT, qT_old); + + +#ifndef NDEBUG + image3d<value::rgb8> tmp_ = duplicate(debug); + mln_piter(p_array<P>) p_dbg(P_); + for_all(p_dbg) + tmp_(qR_old.rotate(p_dbg.to_vec()) + qT_old) = literal::green; + std::ostringstream ss; + ss << "tmp_0"; + if (k < 10) + ss << "0"; + ss << k << ".ppm"; + io::ppm::save(mln::slice(tmp_,0), ss.str()); +#endif + + std::cout << "e_" << k << "=" << e_k << std::endl; + std::cout << "d_" << k << "=" << d_k << std::endl; + + // Check distance and error according to the related paper. + // Disabled because of the following 'if' +// mln_assertion(0 <= d_k); +// mln_assertion(d_k <= e_k); + +// mln_assertion(e_k <= d_k_old); +// mln_assertion(d_k_old <= e_k_old); + + // During the first runs, d_k may be higher than e_k. + // Hence, there we test k > 3. + if (k > 3 && (d_k > e_k || d_k > d_k_old || e_k > e_k_old)) + { + qR = qR_old; + qT = qT_old; + break; + } + + // Backing up results. + d_k_old = d_k; + e_k_old = e_k; + + ++k; + + } while ((k < 3) + || norm::l2((qT - qT_old)) + norm::l2((qR - qR_old).to_vec()) > 1e-3); + + trace::exiting("registration::icp"); + return std::make_pair(qR, qT); } + # endif // ! MLN_INCLUDE_ONLY } // end of namespace mln::registration } // end of namespace mln - - #endif // ! MLN_REGISTRATION_ICP_HH diff --git a/milena/mln/registration/icp2.hh b/milena/mln/registration/icp2.hh deleted file mode 100644 index de763ea..0000000 --- a/milena/mln/registration/icp2.hh +++ /dev/null @@ -1,693 +0,0 @@ -// Copyright (C) 2009 EPITA Research and Development Laboratory (LRDE) -// -// This file is part of the Olena Library. This library is free -// software; you can redistribute it and/or modify it under the terms -// of the GNU General Public License version 2 as published by the -// Free Software Foundation. -// -// This library is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -// General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with this library; see the file COPYING. If not, write to -// the Free Software Foundation, 51 Franklin Street, Fifth Floor, -// Boston, MA 02111-1307, USA. -// -// As a special exception, you may use this file as part of a free -// software library without restriction. Specifically, if other files -// instantiate templates or use macros or inline functions from this -// file, or you compile this file and link it with other files to -// produce an executable, this file does not by itself cause the -// resulting executable to be covered by the GNU General Public -// License. This exception does not however invalidate any other -// reasons why the executable file might be covered by the GNU General -// Public License. - -#ifndef MLN_REGISTRATION_ICP_HH -# define MLN_REGISTRATION_ICP_HH - -/// \file mln/registration/icp.hh -/// -/// Register an image over an another using the ICP algorithm. -/// -/// \todo encode distances on 12 bits. - -# include <cmath> -# include <algorithm> - -# include <mln/core/alias/vec3d.hh> -# include <mln/math/jacobi.hh> -# include <mln/fun/x2x/all.hh> -# include <mln/fun/x2v/all.hh> -# include <mln/convert/to.hh> -# include <mln/accu/compute.hh> -# include <mln/accu/center.hh> -# include <mln/accu/rms.hh> -# include <mln/trait/image_from_grid.hh> -# include <mln/set/compute.hh> - -//Should be removed when closest_point functors are moved. -# include <mln/core/alias/neighb3d.hh> -# include <mln/transform/internal/closest_point_functor.hh> -# include <mln/canvas/distance_geodesic.hh> -# include <mln/pw/all.hh> - -# include <mln/io/ppm/save.hh> -# include <mln/io/pbm/save.hh> -# include <mln/debug/colorize.hh> - -# include <mln/literal/black.hh> -# include <mln/literal/white.hh> -# include <mln/literal/colors.hh> - -# include <mln/core/image/slice_image.hh> - -#include <mln/core/image/tr_image.hh> -#include <mln/core/image/extension_fun.hh> - -#include <mln/accu/histo.hh> -#include <mln/accu/sum.hh> -#include <mln/debug/histo.hh> - -# include <mln/util/timer.hh> - -namespace mln -{ - - - namespace registration - { - - //FIXME: used for debug purpose. Should be removed later. - - using namespace fun::x2x; - - /*! Register point in \p c using a function of closest points - * \p closest_point. - * - * \param[in] P_ The cloud of points. - * \param[in] X the reference surface. - * \param[in] closest_point The function of closest points. - * \param[out] qk The rigid transformation obtained. - * - * \return the rigid transformation which may be use later to create - * a registered image. - * - * WARNING: the function \p closest_point *MUST* take float/double - * vector as arguments. Otherwise the resulting transformation may be - * wrong due to the truncation of the vector coordinate values. - * - * \pre \p P_ and \p X must not be empty. - * - * Reference article: "A Method for Registration of 3-D Shapes", Paul J. - * Besl and Neil D. McKay, IEEE, 2, February 1992. - * - */ - template <typename P, typename F> - std::pair<algebra::quat,mln_vec(P)> - icp(const p_array<P>& P_, - const p_array<P>& X, - const F& closest_point, - const algebra::quat& initial_rot, - const mln_vec(P)& initial_translation); - - - template <typename P, typename F> - composed< translation<P::dim,float>,rotation<P::dim,float> > - icp(const p_array<P>& P_, - const p_array<P>& X, - const F& closest_point); - - -# ifndef MLN_INCLUDE_ONLY - - - /// Closest point functor based on map distance. - template <typename P> - class closest_point_with_map - { - typedef mln_image_from_grid(mln_grid(P), P) I; - typedef mln_ch_value(I, P) cp_ima_t; - typedef mln_ch_value(I,value::int_u16) dmap_t; - - public: - - closest_point_with_map(const p_array<P>& X) - { - box3d box = geom::bbox(X); - box.enlarge(1, box.nrows()); - box.enlarge(2, box.ncols()); - - mln_postcondition(box.is_valid()); - - std::cout << "Map image defined on " << box << std::endl; - - init(X, box); - } - - closest_point_with_map(const p_array<P>& X, const box<P>& box) - { - init(X, box); - } - - void init(const p_array<P>& X, const box<P>& box) - { - typedef mln_ch_value(I, bool) model_t; - model_t model(box); - data::fill(model, false); - data::fill((model | X).rw(), true); - - transform::internal::closest_point_functor<model_t> f; - util::timer t; - t.start(); - dmap_X_ = canvas::distance_geodesic(model, c6(), - mln_max(value::int_u16), - f); - std::cout << "canvas::distance_geodesic - " << t << "s" << std::endl; - - initialize(this->cp_ima_, f.cp_ima); - this->cp_ima_ = f.cp_ima; - - mln_postcondition(cp_ima_.is_valid()); - mln_postcondition(cp_ima_.domain().is_valid()); - std::cout << "pmin = " << cp_ima_.domain().pmin() << std::endl;; - std::cout << "pmax = " << cp_ima_.domain().pmax() << std::endl;; - -#ifndef NDEBUG -// mln_ch_value(I, bool) debug2(box); -// data::fill(debug2, false); -// mln_ch_value(I, value::rgb8) debug(box); -// mln_piter(p_array<P>) p(X); -// for_all(p) -// { -// debug(p) = debug::internal::random_color(value::rgb8()); -// debug2(p) = true; -// } -// io::pbm::save(slice(debug2,0), "debug2-a.ppm"); -// -// mln_piter(I) pi(cp_ima_.domain()); -// for_all(pi) -// { -// debug(pi) = debug(cp_ima_(pi)); -// debug2(pi) = debug2(cp_ima_(pi)); -// } -// -// io::pbm::save(slice(debug2,0), "debug2-b.ppm"); -// io::ppm::save(slice(debug,0), "debug.ppm"); -// std::cout << "map saved" << std::endl; -#endif - } - - mln_site(I) operator()(const mln_site(I)& p) const - { - return cp_ima_(p); - } - - // Distance map - dmap_t dmap_X_; - // Closest point image. - cp_ima_t cp_ima_; - private: - }; - - - /// Closest point functor based on map distance. - template <typename P> - class closest_point_basic - { - typedef mln_image_from_grid(mln_grid(P), P) I; - typedef p_array<P> X_t; - - public: - - closest_point_basic(const p_array<P>& X) - : X_(X) - { - } - - mln_site(I) operator()(const vec3d_f& v) const - { - vec3d_f best_x = convert::to<vec3d_f>(X_[0].to_vec()); - - float best_d = norm::l2_distance(v, best_x); - mln_piter(X_t) X_i(X_); - for_all(X_i) - { - float d = norm::l2_distance(v, convert::to<vec3d_f>(X_i)); - if (d < best_d) - { - best_d = d; - best_x = X_i.to_vec(); - } - } - return best_x; - } - - private: - const p_array<P>& X_; - }; - - - template <typename P> - void - draw_last_run(const box3d& box, const p_array<P>& kept, - const p_array<P>& removed, const p_array<P>& X, - const algebra::quat& qR, const vec3d_f qT) - { - typedef image3d<value::rgb8> result_t; - result_t result(box); - typedef extension_fun<result_t,pw::cst_<mln_value(result_t)> > ext_result_t; - ext_result_t ext_result(result, pw::cst(value::rgb8(0,0,0))); - - data::fill(ext_result, literal::black); - data::fill((ext_result | X).rw(), literal::white); - - mln_piter(p_array<P>) p(kept); - for_all(p) - ext_result(qR.rotate(p.to_vec()) + qT) = literal::green; - - mln_piter(p_array<P>) p2(removed); - for_all(p2) - ext_result(qR.rotate(p2.to_vec()) + qT) = literal::red; - - io::ppm::save(slice(ext_result,0), "registered-2.ppm"); - } - - - - template <typename P, typename F> - float compute_standard_deviation(const p_array<P>& P_, - const std::pair<algebra::quat,mln_vec(P)>& pair, - const F& closest_point) - { - accu::rms<vec3d_f,float> e_k_accu; - - // Standard Deviation - float sd; - mln_piter(p_array<P>) p(P_); - for_all(p) - { - vec3d_f Pk_i = pair.first.rotate(p.to_vec()) + pair.second; - vec3d_f Yk_i = closest_point(Pk_i).to_vec(); - // yk_i - pk_i - e_k_accu.take(Yk_i - Pk_i); - } - - float d = e_k_accu.to_result(); - sd = std::sqrt(e_k_accu.hook_value_() / P_.nsites() - d * d); - return sd; - } - - - template <typename P, typename F> - void - remove_too_far_sites_debug(image3d<value::rgb8>& out, const p_array<P>& P_, - const F& closest_point, - const std::pair<algebra::quat,mln_vec(P)>& pair, - const p_array<P>& X, - unsigned r, int d_min, int d_max, unsigned prefix) - { - unsigned removed = 0; - accu::histo<value::int_u8> h; - mln_piter(p_array<P>) p(P_); - data::fill(out, literal::black); - data::fill((out | X).rw(), literal::white); - - for_all(p) - { - vec3d_f Pk_i = pair.first.rotate(p.to_vec()) + pair.second; - vec3d_f Yk_i = closest_point(Pk_i).to_vec(); - - int d_i = closest_point.dmap_X_(Pk_i); - if (d_i >= d_min && d_i <= d_max) - out(Pk_i) = literal::green; - else - { - ++removed; - out(Pk_i) = literal::red; - } - } - -#ifndef NDEBUG - std::ostringstream ss1; - ss1 << "histo_" << prefix << r << ".dat"; - std::cout << h << std::endl; - - std::ostringstream ss2; - ss2 << "out_" << prefix << r << ".ppm"; - io::ppm::save(mln::slice(out,0), ss2.str()); -#endif - std::cout << "Points removed with the whole set and current d_min/d_max: " << removed << std::endl; - - } - - - template <typename P, typename F> - void - compute_distance_criteria(const p_array<P>& P_, - const F& closest_point, - const std::pair<algebra::quat,mln_vec(P)>& pair, - unsigned r, int& d_min, int& d_max) - { - mln_piter(p_array<P>) p(P_); - accu::histo<value::int_u8> h; - - float sd; - { - accu::sum<float> s, s2; - for_all(p) - { - vec3d_f Pk_i = pair.first.rotate(p.to_vec()) + pair.second; - unsigned d_i = closest_point.dmap_X_(Pk_i); - h.take(d_i); - s.take(d_i); - s2.take(d_i * d_i); - } - float mean = s / P_.nsites(); - sd = std::sqrt(s2 / P_.nsites() - mean * mean); - d_min = int(mean - sd); - d_max = int(mean + sd); - } - - std::cout << "Standard deviation = " << sd << std::endl; - std::ostringstream ss1; - ss1 << "histo_" << r << ".dat"; - std::cout << h << std::endl; - std::cout << "d thresholds = " << d_min << ' ' << d_max << std::endl; - } - - template <typename P, typename F> - p_array<P> - remove_too_far_sites(image3d<value::rgb8>& out, const p_array<P>& P_, - const F& closest_point, - const std::pair<algebra::quat,mln_vec(P)>& pair, - const p_array<P>& X, p_array<P>& removed_set, - unsigned r, int d_min, int d_max, - const std::string& method) - { - p_array<P> tmp; - unsigned removed = 0; - -# ifndef NDEBUG - data::fill(out, literal::black); - data::fill((out | X).rw(), literal::white); -# endif // ! NDEBUG - - mln_piter(p_array<P>) p(P_); - for_all(p) - { - vec3d_f Pk_i = pair.first.rotate(p.to_vec()) + pair.second; - vec3d_f Yk_i = closest_point(Pk_i).to_vec(); - - int d_i = closest_point.dmap_X_(Pk_i); - if (d_i >= d_min && d_i <= d_max) - { - tmp.append(p); - out(Pk_i) = literal::green; - } - else - { - ++removed; - removed_set.append(p); - out(Pk_i) = literal::red; - } - } - - -# ifndef NDEBUG - std::ostringstream ss2; - ss2 << method << "_" << r << "_removed_sites" << ".ppm"; - io::ppm::save(mln::slice(out,0), ss2.str()); - - std::cout << "Points removed: " << removed << std::endl; -# endif // ! NDEBUG - // They are used for debug purpose only. - // When NDEBUG is set, they are unused. - (void) X; - (void) r; - (void) method; - - return tmp; - } - - template <typename P> - void - display_sites_used_in_icp(image3d<value::rgb8>& out, const p_array<P>& P_sub, - const p_array<P>& P_, const p_array<P>& X, - unsigned r, const std::string& prefix, - const std::pair<algebra::quat,mln_vec(P)>& pair, - const std::string& period, const value::rgb8& c) - { -# ifndef NDEBUG - data::fill(out, literal::black); - data::fill((out | X).rw(), literal::white); -# endif // !NDEBUG - - mln_piter(p_array<P>) p1(P_); - for_all(p1) - { - vec3d_f Pk_i = pair.first.rotate(p1.to_vec()) + pair.second; - out(Pk_i) = literal::red; - } - - mln_piter(p_array<P>) p2(P_sub); - for_all(p2) - { - vec3d_f Pk_i = pair.first.rotate(p2.to_vec()) + pair.second; - out(Pk_i) = c; - } - -# ifndef NDEBUG - std::ostringstream ss; - ss << prefix << "_" << r << "_" << period << ".ppm"; - - io::ppm::save(slice(out,0), ss.str()); -# endif // ! NDEBUG - } - - - template <typename P, typename F> - inline - float - compute_d_k(const p_array<P>& P_, - const F& closest_point, - const algebra::quat& qR, - const algebra::quat& qR_old, - const vec3d_f& qT, - const vec3d_f& qT_old) - { - accu::rms<vec3d_f, float> accu; - mln_piter(p_array<P>) p(P_); - for_all(p) - { - // yk_i - pk+1_i - vec3d_f Pk_i = qR_old.rotate(p.to_vec()) + qT_old; - vec3d_f Pk_1_i = qR.rotate(p.to_vec()) + qT; - accu.take(closest_point(Pk_i).to_vec() - Pk_1_i); - } - return accu.to_result(); - } - - - /// FIXME: work only for 3d images. - template <typename P, typename F> - algebra::quat - get_rot(const p_array<P>& P_, - const vec3d_f& mu_P, - const vec3d_f& mu_Yk, - const F& closest_point, - const algebra::quat& qR, - const vec3d_f& qT) - { - /// Spx: cross-covariance matrix. - algebra::mat<3u,3u,float> Spx; - mln_piter(p_array<P>) p(P_); - - // FIXME: could we use an accu? - for_all(p) - { - vec3d_f P_i = p.to_vec(); - vec3d_f Pk_i = qR.rotate(p.to_vec()) + qT; - vec3d_f Yk_i = closest_point(Pk_i).to_vec(); - Spx += make::mat(P_i - mu_P) * trans(make::mat(Yk_i - mu_Yk)); - } - Spx /= P_.nsites(); - - vec3d_f A; - A[0] = Spx(1,2) - Spx(2,1); - A[1] = Spx(2,0) - Spx(0,2); - A[2] = Spx(0,1) - Spx(1,0); - - algebra::mat<4u,4u,float> Qk; - float t = tr(Spx); - - Qk(0,0) = t; - for (int i = 1; i < 4; ++i) - { - Qk(i,0) = A[i - 1]; - Qk(0,i) = A[i - 1]; - for (int j = 1; j < 4; ++j) - if (i == j) - Qk(i,j) = 2 * Spx(i - 1,i - 1) - t; - } - - Qk(1,2) = Spx(0,1) + Spx(1,0); - Qk(2,1) = Spx(0,1) + Spx(1,0); - - Qk(1,3) = Spx(0,2) + Spx(2,0); - Qk(3,1) = Spx(0,2) + Spx(2,0); - - Qk(2,3) = Spx(1,2) + Spx(2,1); - Qk(3,2) = Spx(1,2) + Spx(2,1); - - return math::jacobi(Qk); - } - - - // Compute mu_Yk, mass center of Yk. - template <typename P, typename F> - inline - vec3d_f - get_mu_yk(const p_array<P>& P_, - const F& closest_point, - const algebra::quat& qR, - const vec3d_f& qT, - float& e_k) - { - accu::rms<vec3d_f,float> e_k_accu; - accu::center<P,vec3d_f> mu_yk; - - mln_piter(p_array<P>) p(P_); - for_all(p) - { - // yk_i - pk_i - vec3d_f Pk_i = qR.rotate(p.to_vec()) + qT; - vec3d_f Yk_i = closest_point(Pk_i).to_vec(); - mu_yk.take(Yk_i); - e_k_accu.take(Yk_i - Pk_i); - } - - e_k = e_k_accu.to_result(); - return mu_yk.to_result(); - } - - - - /// Base version of the ICP algorithm. It is called in other variants. - template <typename P, typename F> - inline - std::pair<algebra::quat,mln_vec(P)> - icp(const p_array<P>& P_, - const p_array<P>& X, - const F& closest_point, - const algebra::quat& initial_rot, - const mln_vec(P)& initial_translation) - { - trace::entering("registration::icp"); - - (void) X; - mln_precondition(P::dim == 3); - mln_precondition(!P_.is_empty()); - mln_precondition(!X.is_empty()); - mln_precondition(!initial_rot.is_null()); - - typedef p_array<P> cloud_t; - - vec3d_f mu_P = set::compute(accu::center<P,vec3d_f>(), P_); - - vec3d_f qT_old, qT = initial_translation; - algebra::quat qR_old, qR = initial_rot; - float e_k, e_k_old = mln_max(float); - float d_k, d_k_old = mln_max(float); - unsigned k = 0; - -# ifndef NDEBUG - box3d box = geom::bbox(X); - //FIXME: too large? - box.enlarge(1, box.nrows() / 2); - box.enlarge(2, box.ncols() / 2); - image3d<value::rgb8> debug(box); - data::fill(debug, literal::black); - data::fill((debug | X).rw(), literal::white); -# endif - - do - { - qT_old = qT; - qR_old = qR; - - /// Compute transformation - /// - // mu_Yk - Pk's mass center. - // + Compute error ek = d(Pk,Yk) - vec3d_f mu_Yk = get_mu_yk(P_, closest_point, qR_old, qT_old, e_k); - - // quaternion qR - rotation - qR = get_rot(P_, mu_P, mu_Yk, closest_point, qR_old, qT_old); - vec3d_f tmp = qR.v(); - - // vector qT - translation - qT = mu_Yk - qR.rotate(mu_P); - /// - /// End of "compute transformation" - - // Distance dk = d(Pk+1, Yk) - d_k = compute_d_k(P_, closest_point, qR, qR_old, qT, qT_old); - - -#ifndef NDEBUG - image3d<value::rgb8> tmp_ = duplicate(debug); - mln_piter(p_array<P>) p_dbg(P_); - for_all(p_dbg) - tmp_(qR_old.rotate(p_dbg.to_vec()) + qT_old) = literal::green; - std::ostringstream ss; - ss << "tmp_0"; - if (k < 10) - ss << "0"; - ss << k << ".ppm"; - io::ppm::save(mln::slice(tmp_,0), ss.str()); -#endif - - std::cout << "e_" << k << "=" << e_k << std::endl; - std::cout << "d_" << k << "=" << d_k << std::endl; - - // Check distance and error according to the related paper. - // Disabled because of the following 'if' -// mln_assertion(0 <= d_k); -// mln_assertion(d_k <= e_k); - -// mln_assertion(e_k <= d_k_old); -// mln_assertion(d_k_old <= e_k_old); - - // During the first runs, d_k may be higher than e_k. - // Hence, there we test k > 3. - if (k > 3 && (d_k > e_k || d_k > d_k_old || e_k > e_k_old)) - { - qR = qR_old; - qT = qT_old; - break; - } - - // Backing up results. - d_k_old = d_k; - e_k_old = e_k; - - ++k; - - } while ((k < 3) - || norm::l2((qT - qT_old)) + norm::l2((qR - qR_old).to_vec()) > 1e-3); - - trace::exiting("registration::icp"); - return std::make_pair(qR, qT); - } - - -# endif // ! MLN_INCLUDE_ONLY - - } // end of namespace mln::registration - -} // end of namespace mln - -#endif // ! MLN_REGISTRATION_ICP_HH diff --git a/milena/mln/registration/registration.hh b/milena/mln/registration/registration.hh index f96787a..21d6ea5 100644 --- a/milena/mln/registration/registration.hh +++ b/milena/mln/registration/registration.hh @@ -34,11 +34,14 @@ /// \sa registration::icp # include <mln/core/image/image3d.hh> -# include <mln/registration/icp2.hh> +# include <mln/registration/icp.hh> # include <mln/fun/x2x/all.hh> # include <mln/fun/x2p/closest_point.hh> # include <mln/convert/to_p_array.hh> +//FIXME: to be removed. +# include <mln/util/timer.hh> + namespace mln { @@ -331,7 +334,7 @@ namespace mln { trace::entering("registration::registration1"); - registration_tests(cloud, surface); + internal::registration_tests(cloud, surface); composed< translation<P::dim,float>, rotation<P::dim,float> > qk = impl::registration1(cloud, surface); @@ -350,7 +353,7 @@ namespace mln { trace::entering("registration::registration2"); - registration_tests(cloud, surface); + internal::registration_tests(cloud, surface); composed< translation<P::dim,float>, rotation<P::dim,float> > qk = impl::registration2(cloud, surface); diff --git a/milena/mln/util/graph.hh b/milena/mln/util/graph.hh index f95e1e8..10e900f 100644 --- a/milena/mln/util/graph.hh +++ b/milena/mln/util/graph.hh @@ -405,7 +405,9 @@ namespace mln unsigned id = data_->edges_.size() - 1; // Update the set of edges. +# ifndef NDEBUG data_->edges_set_.insert(edge); +# endif // ! NDEBUG data_->vertices_[edge.first()].push_back(id); data_->vertices_[edge.second()].push_back(id); diff --git a/milena/mln/value/rgb.hh b/milena/mln/value/rgb.hh index 9c64936..839d89c 100644 --- a/milena/mln/value/rgb.hh +++ b/milena/mln/value/rgb.hh @@ -51,6 +51,11 @@ namespace mln { + // Forward declaration. + namespace value { template <unsigned n> struct rgb; } + + + namespace fun { -- 1.5.6.5