[Olena-patches] 3405: Update ICP.

* mln/registration/icp.hh: store point indexes instead of points in closest point image. Remove useless to_vec(). * mln/registration/registration.hh: remove useless to_vec(). Pass a domain to the function to be used to compute the closest point image. * mln/transform/internal/closest_point_functor.hh: Store point indexes instead of points. --- milena/ChangeLog | 13 +++ milena/mln/registration/icp.hh | 81 +++++++++++--------- milena/mln/registration/registration.hh | 63 ++++++++-------- .../transform/internal/closest_point_functor.hh | 29 ++++++- 4 files changed, 114 insertions(+), 72 deletions(-) diff --git a/milena/ChangeLog b/milena/ChangeLog index ce02eb9..4efb183 100644 --- a/milena/ChangeLog +++ b/milena/ChangeLog @@ -1,5 +1,18 @@ 2009-02-20 Guillaume Lazzara &lt;z(a)lrde.epita.fr&gt; + Update ICP. + + * mln/registration/icp.hh: store point indexes instead of points in + closest point image. Remove useless to_vec(). + + * mln/registration/registration.hh: remove useless to_vec(). Pass a + domain to the function to be used to compute the closest point image. + + * mln/transform/internal/closest_point_functor.hh: Store point indexes + instead of points. + +2009-02-20 Guillaume Lazzara &lt;z(a)lrde.epita.fr&gt; + Reorder data in the vector returned by to_vec(). * mln/core/concept/gdpoint.hh, diff --git a/milena/mln/registration/icp.hh b/milena/mln/registration/icp.hh index de763ea..d484f5c 100644 --- a/milena/mln/registration/icp.hh +++ b/milena/mln/registration/icp.hh @@ -49,28 +49,31 @@ # include <mln/set/compute.hh> //Should be removed when closest_point functors are moved. +# include <mln/core/image/slice_image.hh> +# include <mln/core/image/tr_image.hh> +# include <mln/core/image/extension_fun.hh> + # 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/debug/histo.hh> + +# include <mln/accu/histo.hh> +# include <mln/accu/sum.hh> + +# include <mln/value/int_u16.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 @@ -129,7 +132,7 @@ namespace mln 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, unsigned) cp_ima_t; typedef mln_ch_value(I,value::int_u16) dmap_t; public: @@ -137,6 +140,7 @@ namespace mln closest_point_with_map(const p_array<P>& X) { box3d box = geom::bbox(X); + box.enlarge(0, box.nslis()); box.enlarge(1, box.nrows()); box.enlarge(2, box.ncols()); @@ -144,11 +148,13 @@ namespace mln std::cout << "Map image defined on " << box << std::endl; + X_ = X; init(X, box); } closest_point_with_map(const p_array<P>& X, const box<P>& box) { + X_ = X; init(X, box); } @@ -159,13 +165,14 @@ namespace mln data::fill(model, false); data::fill((model | X).rw(), true); - transform::internal::closest_point_functor<model_t> f; + transform::internal::closest_point_functor<model_t> f(X); + 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; + std::cout << "distance_geodesic = " << t << "s" << std::endl; initialize(this->cp_ima_, f.cp_ima); this->cp_ima_ = f.cp_ima; @@ -202,14 +209,18 @@ namespace mln mln_site(I) operator()(const mln_site(I)& p) const { - return cp_ima_(p); + return X_[cp_ima_(p)]; } + // Distance map dmap_t dmap_X_; + + private: + p_array<P> X_; // Closest point image. cp_ima_t cp_ima_; - private: + }; @@ -229,17 +240,18 @@ namespace mln mln_site(I) operator()(const vec3d_f& v) const { - vec3d_f best_x = convert::to<vec3d_f>(X_[0].to_vec()); + vec3d_f best_x = X_[0]; 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)); + vec3d_f X_i_vec = X_i; + float d = norm::l2_distance(v, X_i_vec); if (d < best_d) { best_d = d; - best_x = X_i.to_vec(); + best_x = X_i_vec; } } return best_x; @@ -266,11 +278,11 @@ namespace mln mln_piter(p_array<P>) p(kept); for_all(p) - ext_result(qR.rotate(p.to_vec()) + qT) = literal::green; + ext_result(qR.rotate(p) + qT) = literal::green; mln_piter(p_array<P>) p2(removed); for_all(p2) - ext_result(qR.rotate(p2.to_vec()) + qT) = literal::red; + ext_result(qR.rotate(p2) + qT) = literal::red; io::ppm::save(slice(ext_result,0), "registered-2.ppm"); } @@ -289,7 +301,7 @@ namespace mln mln_piter(p_array<P>) p(P_); for_all(p) { - vec3d_f Pk_i = pair.first.rotate(p.to_vec()) + pair.second; + vec3d_f Pk_i = pair.first.rotate(p) + pair.second; vec3d_f Yk_i = closest_point(Pk_i).to_vec(); // yk_i - pk_i e_k_accu.take(Yk_i - Pk_i); @@ -317,8 +329,8 @@ namespace mln 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(); + vec3d_f Pk_i = pair.first.rotate(p) + pair.second; + vec3d_f Yk_i = closest_point(Pk_i); int d_i = closest_point.dmap_X_(Pk_i); if (d_i >= d_min && d_i <= d_max) @@ -399,7 +411,7 @@ namespace mln 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(); + vec3d_f Yk_i = closest_point(Pk_i); int d_i = closest_point.dmap_X_(Pk_i); if (d_i >= d_min && d_i <= d_max) @@ -440,31 +452,27 @@ namespace mln 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; + vec3d_f Pk_i = pair.first.rotate(p1) + 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; + vec3d_f Pk_i = pair.first.rotate(p2) + 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 } @@ -483,8 +491,9 @@ namespace mln 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; + vec3d_f P_i = p; + vec3d_f Pk_i = qR_old.rotate(P_i) + qT_old; + vec3d_f Pk_1_i = qR.rotate(P_i) + qT; accu.take(closest_point(Pk_i).to_vec() - Pk_1_i); } return accu.to_result(); @@ -508,9 +517,9 @@ namespace mln // 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(); + vec3d_f P_i = p; + vec3d_f Pk_i = qR.rotate(P_i) + qT; + vec3d_f Yk_i = closest_point(Pk_i); Spx += make::mat(P_i - mu_P) * trans(make::mat(Yk_i - mu_Yk)); } Spx /= P_.nsites(); @@ -564,7 +573,7 @@ namespace mln { // yk_i - pk_i vec3d_f Pk_i = qR.rotate(p.to_vec()) + qT; - vec3d_f Yk_i = closest_point(Pk_i).to_vec(); + vec3d_f Yk_i = closest_point(Pk_i); mu_yk.take(Yk_i); e_k_accu.take(Yk_i - Pk_i); } @@ -641,7 +650,7 @@ namespace mln 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; + tmp_(qR_old.rotate(p_dbg) + qT_old) = literal::green; std::ostringstream ss; ss << "tmp_0"; if (k < 10) diff --git a/milena/mln/registration/registration.hh b/milena/mln/registration/registration.hh index 21d6ea5..98c5ae9 100644 --- a/milena/mln/registration/registration.hh +++ b/milena/mln/registration/registration.hh @@ -34,6 +34,7 @@ /// \sa registration::icp # include <mln/core/image/image3d.hh> +# include <mln/core/site_set/box.hh> # include <mln/registration/icp.hh> # include <mln/fun/x2x/all.hh> # include <mln/fun/x2p/closest_point.hh> @@ -56,7 +57,8 @@ namespace mln template <typename P> inline composed< translation<P::dim,float>,rotation<P::dim,float> > - registration1(const p_array<P>& P_, + registration1(const box<P>& domain, + const p_array<P>& P_, const p_array<P>& X); //FIXME: move to registration.hh @@ -70,7 +72,8 @@ namespace mln template <typename P> inline composed< translation<P::dim,float>,rotation<P::dim,float> > - registration2(const p_array<P>& P_, + registration2(const box<P>& domain, + const p_array<P>& P_, const p_array<P>& X); //FIXME: move to registration.hh @@ -83,7 +86,8 @@ namespace mln template <typename P> inline composed< translation<P::dim,float>,rotation<P::dim,float> > - registration3(const p_array<P>& P_, + registration3(const box<P>& domain, + const p_array<P>& P_, const p_array<P>& X); @@ -119,8 +123,9 @@ namespace mln template <typename P> inline composed< translation<P::dim,float>,rotation<P::dim,float> > - registration1(const p_array<P>& P_, - const p_array<P>& X) + registration1(const box<P>& domain, + const p_array<P>& P_, + const p_array<P>& X) { trace::entering("mln::registration::registration1"); @@ -129,7 +134,7 @@ namespace mln t.start(); # endif // ! NDEBUG - registration::closest_point_with_map<P> closest_point(X); + registration::closest_point_with_map<P> closest_point(X, domain); std::pair<algebra::quat,mln_vec(P)> pair = icp(P_, X, closest_point, algebra::quat(1,0,0,0), @@ -153,15 +158,16 @@ namespace mln template <typename P> inline composed< translation<P::dim,float>,rotation<P::dim,float> > - registration2(const p_array<P>& P_, - const p_array<P>& X) + registration2(const box<P>& domain, + const p_array<P>& P_, + const p_array<P>& X) { trace::entering("mln::registration::registration2"); // Used for debug. std::string method = "registration2"; - registration::closest_point_with_map<P> closest_point(X); + registration::closest_point_with_map<P> closest_point(X, domain); # ifndef NDEBUG util::timer t; @@ -175,12 +181,9 @@ namespace mln std::pair<algebra::quat,mln_vec(P)> pair; pair.first = algebra::quat(1,0,0,0); pair.second = literal::zero; - box3d box = geom::bbox(X); - box.enlarge(1, 60); - box.enlarge(2, 60); // Used for debug. - image3d<value::rgb8> out(box); + image3d<value::rgb8> out(domain); p_array<P> removed_set; @@ -219,7 +222,7 @@ namespace mln # ifndef NDEBUG std::cout << "icp = " << t << std::endl; - draw_last_run(box, P_bak, removed_set, X, pair.first, pair.second); + draw_last_run(domain, P_bak, removed_set, X, pair.first, pair.second); # endif typedef rotation<3u,float> rot_t; @@ -237,15 +240,13 @@ namespace mln template <typename P> inline composed< translation<P::dim,float>,rotation<P::dim,float> > - registration3(const p_array<P>& P_, - const p_array<P>& X) + registration3(const box<P>& domain, + const p_array<P>& P_, + const p_array<P>& X) { trace::entering("mln::registration::registration3"); - registration::closest_point_with_map<P> closest_point(X); - std::cout << " pmin and pmax: " << std::endl; - std::cout << closest_point.cp_ima_.domain().pmin() << std::endl; - std::cout << closest_point.cp_ima_.domain().pmax() << std::endl; + registration::closest_point_with_map<P> closest_point(X, domain); // Used for debug. std::string method = "registration3"; @@ -262,12 +263,9 @@ namespace mln std::pair<algebra::quat,mln_vec(P)> pair; pair.first = algebra::quat(1,0,0,0); pair.second = literal::zero; - box3d box = geom::bbox(X); - box.enlarge(1, 60); - box.enlarge(2, 60); // Used for debug. - image3d<value::rgb8> out(box); + image3d<value::rgb8> out(domain); p_array<P> removed_set; @@ -306,7 +304,7 @@ namespace mln # ifndef NDEBUG std::cout << "icp = " << t << std::endl; - draw_last_run(box, P_bak, removed_set, X, pair.first, pair.second); + draw_last_run(domain, P_bak, removed_set, X, pair.first, pair.second); # endif // ! NDEBUG typedef rotation<3u,float> rot_t; @@ -329,7 +327,8 @@ namespace mln template <typename P> inline composed< translation<P::dim,float>,rotation<P::dim,float> > - registration1(const p_array<P>& cloud, + registration1(const box<P>& domain, + const p_array<P>& cloud, const p_array<P>& surface) { trace::entering("registration::registration1"); @@ -337,7 +336,7 @@ namespace mln internal::registration_tests(cloud, surface); composed< translation<P::dim,float>, rotation<P::dim,float> > - qk = impl::registration1(cloud, surface); + qk = impl::registration1(domain, cloud, surface); trace::exiting("registration::registration1"); @@ -348,7 +347,8 @@ namespace mln template <typename P> inline composed< translation<P::dim,float>,rotation<P::dim,float> > - registration2(const p_array<P>& cloud, + registration2(const box<P>& domain, + const p_array<P>& cloud, const p_array<P>& surface) { trace::entering("registration::registration2"); @@ -356,7 +356,7 @@ namespace mln internal::registration_tests(cloud, surface); composed< translation<P::dim,float>, rotation<P::dim,float> > - qk = impl::registration2(cloud, surface); + qk = impl::registration2(domain, cloud, surface); trace::exiting("registration::registration2"); @@ -367,7 +367,8 @@ namespace mln template <typename P> inline composed< translation<P::dim,float>,rotation<P::dim,float> > - registration3(const p_array<P>& cloud, + registration3(const box<P>& domain, + const p_array<P>& cloud, const p_array<P>& surface) { trace::entering("registration::registration3"); @@ -375,7 +376,7 @@ namespace mln internal::registration_tests(cloud, surface); composed< translation<P::dim,float>, rotation<P::dim,float> > - qk = impl::registration3(cloud, surface); + qk = impl::registration3(domain, cloud, surface); trace::exiting("registration::registration3"); diff --git a/milena/mln/transform/internal/closest_point_functor.hh b/milena/mln/transform/internal/closest_point_functor.hh index 99f8391..098c994 100644 --- a/milena/mln/transform/internal/closest_point_functor.hh +++ b/milena/mln/transform/internal/closest_point_functor.hh @@ -50,7 +50,10 @@ namespace mln typedef mln_value(I) V; typedef mln_psite(I) P; - mln_ch_value(I,P) cp_ima; + closest_point_functor(const p_array<P>& pset); + + mln_ch_value(I,unsigned) cp_ima; + const p_array<P>& pset_; void init(const I&); bool inqueue_p_wrt_input_p(const V& input_p); @@ -58,9 +61,13 @@ namespace mln bool inqueue_p_wrt_input_n(const V& input_n); void process(const P&, const P&); - void init_(const I& input) { initialize(cp_ima, input); } + void init_(const I& input) + { + init(input); + } + bool inqueue_p_wrt_input_p_(const V& input_p) { return input_p == true; } - void init_p_(unsigned p) { cp_ima.element(p) = cp_ima.point_at_index(p); } + void init_p_(unsigned) { }//cp_ima.element(p) = pset_(cp_ima.point_at_index(p)); } bool inqueue_p_wrt_input_n_(const V& input_n) { return input_n == false; } void process_(unsigned p, unsigned n) { cp_ima.element(n) = cp_ima.element(p); } }; @@ -70,10 +77,22 @@ namespace mln template <typename I> inline + closest_point_functor<I>::closest_point_functor(const p_array<mln_psite(I)>& pset) + : pset_(pset) + { + } + + template <typename I> + inline void closest_point_functor<I>::init(const I& input) { initialize(cp_ima, input); + data::fill(cp_ima, 0u); + + mln_piter(p_array<mln_psite(I)>) p(pset_); + for_all(p) + cp_ima(p) = p.index(); } template <typename I> @@ -87,9 +106,9 @@ namespace mln template <typename I> inline void - closest_point_functor<I>::init_p(const P& p) + closest_point_functor<I>::init_p(const P&) { - cp_ima(p) = p; +// cp_ima(p) = p; } template <typename I> -- 1.5.6.5