[Olena-patches] 3300: Add new ICP variants.

* mln/registration/icp2.hh: Add two new variants. --- milena/ChangeLog | 6 + milena/mln/registration/icp2.hh | 565 +++++++++++++++++++++++++++++++-------- 2 files changed, 457 insertions(+), 114 deletions(-) diff --git a/milena/ChangeLog b/milena/ChangeLog index 8960f74..e6b8418 100644 --- a/milena/ChangeLog +++ b/milena/ChangeLog @@ -1,4 +1,10 @@ 2009-02-05 Guillaume Lazzara &lt;z(a)lrde.epita.fr&gt; + + Add new ICP variants. + + * mln/registration/icp2.hh: Add two new variants. + +2009-02-05 Guillaume Lazzara &lt;z(a)lrde.epita.fr&gt; Small changes in transformation related classes. diff --git a/milena/mln/registration/icp2.hh b/milena/mln/registration/icp2.hh index 2993caa..070b4d9 100644 --- a/milena/mln/registration/icp2.hh +++ b/milena/mln/registration/icp2.hh @@ -32,6 +32,8 @@ /// /// Register an image over an another using the ICP algorithm. +# include <algorithm> + # include <mln/core/alias/vec3d.hh> # include <mln/math/jacobi.hh> # include <mln/fun/x2x/all.hh> @@ -49,6 +51,24 @@ # 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/util/timer.hh> + +#include <mln/core/image/tr_image.hh> +#include <mln/core/image/extension_fun.hh> + +#include <mln/accu/histo.hh> +#include <mln/debug/histo.hh> + + namespace mln { @@ -61,8 +81,8 @@ namespace mln /*! Register point in \p c using a function of closest points * \p closest_point. * - * \param[in] data_P The cloud of points. - * \param[in] model_X the reference surface. + * \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. * @@ -73,54 +93,84 @@ namespace mln * vector as arguments. Otherwise the resulting transformation may be * wrong due to the truncation of the vector coordinate values. * - * \pre \p data_p and \p model_X must not be empty. + * \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> - composed< rotation<P::dim,float>,translation<P::dim,float> > - icp(const p_array<P>& data_P, - const p_array<P>& model_X, +// composed< translation<P::dim,float>,rotation<P::dim,float> > + 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< rotation<P::dim,float>,translation<P::dim,float> > - icp(const p_array<P>& data_P, - const p_array<P>& model_X, + 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 cp_with_map_t + 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_u8) dmap_t; public: - cp_with_map_t(const p_array<P>& model_X) + closest_point_with_map(const p_array<P>& X) { - box3d box = geom::bbox(model_X); - box.enlarge(50); + box3d box = geom::bbox(X); + box.enlarge(1, box.nrows() / 2); + box.enlarge(2, box.ncols() / 2); + std::cout << "Map image defined on " << box << std::endl; typedef mln_ch_value(I, bool) model_t; model_t model(box); data::fill(model, false); - data::fill((model | model_X).rw(), true); + data::fill((model | X).rw(), true); - transform::internal::closest_point_functor<I> f; - typedef mln_ch_value(I,value::int_u16) dmap_t; - dmap_t dmap_X = canvas::distance_geodesic(model, c6(), - mln_max(value::int_u16), - f); + transform::internal::closest_point_functor<model_t> f; + util::timer t; + t.start(); + dmap_X_ = canvas::distance_geodesic(model, c6(), + mln_max(value::int_u8), + f); + std::cout << "canvas::distance_geodesic - " << t << "s" << std::endl; cp_ima_ = f.cp_ima; + +#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 @@ -128,6 +178,7 @@ namespace mln return cp_ima_(p); } + dmap_t dmap_X_; private: cp_ima_t cp_ima_; }; @@ -135,40 +186,27 @@ namespace mln /// Closest point functor based on map distance. template <typename P> - class basic_closest_point + class closest_point_basic { typedef mln_image_from_grid(mln_grid(P), P) I; typedef p_array<P> X_t; public: - basic_closest_point(const p_array<P>& X) + closest_point_basic(const p_array<P>& X) : X_(X) { } - float - l2_distance(const vec3d_f& vec1, const vec3d_f& vec2) const - { - typedef float D; - D d = 0; - for (unsigned i = 0; i < 3; ++i) - { - D sqr_v1_v2 = static_cast<D>(mln::math::sqr(vec1[i] - vec2[i])); - d = static_cast<D>(d + sqr_v1_v2); - } - return d; - } - mln_site(I) operator()(const vec3d_f& v) const { vec3d_f best_x = convert::to<vec3d_f>(X_[0].to_vec()); - float best_d = l2_distance(v, best_x); + float best_d = norm::l2_distance(v, best_x); mln_piter(X_t) X_i(X_); for_all(X_i) { - float d = l2_distance(v, convert::to<vec3d_f>(X_i)); + float d = norm::l2_distance(v, convert::to<vec3d_f>(X_i)); if (d < best_d) { best_d = d; @@ -183,61 +221,146 @@ namespace mln }; + 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> - inline - float - compute_d_k(const p_array<P>& data_P, - const F& closest_point, - const algebra::quat& qR, - const algebra::quat& qR_old, - const vec3d_f& qT, - const vec3d_f& qT_old) + 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> accu; - mln_piter(p_array<P>) p(data_P); + accu::rms<vec3d_f,float> e_k_accu; + + // Standard Deviation + float sd; + 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); + 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); } - return accu.to_result(); + + float d = e_k_accu.to_result(); + sd = math::sqrt((e_k_accu.hook_value_() - 2.5 * math::sqr(d)) / P_.nsites()); + return sd; } + + template <typename P, typename F> + p_array<P> + remove_too_far_sites(image3d<value::rgb8>& out, const p_array<P>& P_bak, + 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) + { + float sd = compute_standard_deviation(P_bak, pair, closest_point); + std::cout << "Standard deviation = " << sd << std::endl; + + p_array<P> tmp; + unsigned removed = 0; + + data::fill(out, literal::white); + data::fill((out | X).rw(), literal::black); + + accu::histo<value::int_u8> h; + mln_piter(p_array<P>) p(P_bak); + 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(); + + float norm = norm::l2_distance(Yk_i, Pk_i); + h.take(closest_point.dmap_X_(Pk_i)); + if (norm < sd && norm > sd / 2) + { + tmp.append(p); + out(Pk_i) = literal::green; + } + else + { + ++removed; + removed_set.append(p); + out(Pk_i) = literal::red; + } + } + + std::ostringstream ss1; + ss1 << "histo_" << r << ".dat"; + debug::histo_plot(h, ss1.str()); + std::cout << h << std::endl; + + std::ostringstream ss2; + ss2 << "out_0" << r << ".ppm"; + io::ppm::save(mln::slice(out,0), ss2.str()); + + std::cout << "Points removed: " << removed << std::endl; + + return tmp; + } + + template <typename P, typename F> inline float - compute_e_k(const p_array<P>& data_P, + compute_d_k(const p_array<P>& P_, const F& closest_point, const algebra::quat& qR, - const vec3d_f& qT) + 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(data_P); + mln_piter(p_array<P>) p(P_); for_all(p) { - // yk_i - pk_i - vec3d_f Pk_i = qR.rotate(p.to_vec()) + qT; - accu.take(closest_point(Pk_i).to_vec() - Pk_i); + // 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>& data_P, + 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) { - /// Mk: cross-covariance matrix. - algebra::mat<3u,3u,float> Mk; - mln_piter(p_array<P>) p(data_P); + /// 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) @@ -245,17 +368,17 @@ namespace mln 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(); - Mk += make::mat(P_i - mu_P) * trans(make::mat(Yk_i - mu_Yk)); + Spx += make::mat(P_i - mu_P) * trans(make::mat(Yk_i - mu_Yk)); } - Mk /= data_P.nsites(); + Spx /= P_.nsites(); vec3d_f A; - A[0] = Mk(1,2) - Mk(2,1); - A[1] = Mk(2,0) - Mk(0,2); - A[2] = Mk(0,1) - Mk(1,0); + 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(Mk); + float t = tr(Spx); Qk(0,0) = t; for (int i = 1; i < 4; ++i) @@ -264,17 +387,17 @@ namespace mln Qk(0,i) = A[i - 1]; for (int j = 1; j < 4; ++j) if (i == j) - Qk(i,j) = 2 * Mk(i - 1,i - 1) - t; + Qk(i,j) = 2 * Spx(i - 1,i - 1) - t; } - Qk(1,2) = Mk(0,1) + Mk(1,0); - Qk(2,1) = Mk(0,1) + Mk(1,0); + Qk(1,2) = Spx(0,1) + Spx(1,0); + Qk(2,1) = Spx(0,1) + Spx(1,0); - Qk(1,3) = Mk(0,2) + Mk(2,0); - Qk(3,1) = Mk(0,2) + Mk(2,0); + Qk(1,3) = Spx(0,2) + Spx(2,0); + Qk(3,1) = Spx(0,2) + Spx(2,0); - Qk(2,3) = Mk(1,2) + Mk(2,1); - Qk(3,2) = Mk(1,2) + Mk(2,1); + Qk(2,3) = Spx(1,2) + Spx(2,1); + Qk(3,2) = Spx(1,2) + Spx(2,1); return mln::math::jacobi(Qk); } @@ -284,63 +407,83 @@ namespace mln template <typename P, typename F> inline vec3d_f - get_mu_yk(const p_array<P>& data_P, + get_mu_yk(const p_array<P>& P_, const F& closest_point, const algebra::quat& qR, - const vec3d_f& qT) + const vec3d_f& qT, + float& e_k) { - accu::center<P,vec3d_f> accu; + accu::rms<vec3d_f,float> e_k_accu; + accu::center<P,vec3d_f> mu_yk; - mln_piter(p_array<P>) p(data_P); + mln_piter(p_array<P>) p(P_); for_all(p) { + // yk_i - pk_i vec3d_f Pk_i = qR.rotate(p.to_vec()) + qT; - accu.take(closest_point(Pk_i).to_vec()); + vec3d_f Yk_i = closest_point(Pk_i).to_vec(); + mu_yk.take(Yk_i); + e_k_accu.take(Yk_i - Pk_i); } - return accu.to_result(); + + 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 - composed< rotation<P::dim,float>,translation<P::dim,float> > - icp(const p_array<P>& data_P, - const p_array<P>& model_X, +// composed< translation<P::dim,float>,rotation<P::dim,float> > + 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(!data_P.is_empty()); - mln_precondition(!model_X.is_empty()); + 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>(), data_P); + 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 ek, ek_old = mln_max(float); - float dk, dk_old = mln_max(float); + 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 error ek = d(Pk,Yk) - ek = compute_e_k(data_P, closest_point, qR, qT); - /// Compute transformation /// // mu_Yk - Pk's mass center. - vec3d_f mu_Yk = get_mu_yk(data_P, closest_point, qR_old, qT_old); + // + 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(data_P, mu_P, mu_Yk, closest_point, qR_old, qT_old); + qR = get_rot(P_, mu_P, mu_Yk, closest_point, qR_old, qT_old); vec3d_f tmp = qR.v(); // vector qT - translation @@ -349,42 +492,236 @@ namespace mln /// End of "compute transformation" // Distance dk = d(Pk+1, Yk) - dk = compute_d_k(data_P, closest_point, qR, qR_old, qT, qT_old); - - // Check property according the related paper. - mln_assertion(0 <= dk); - mln_assertion(dk <= ek); - mln_assertion(ek <= dk_old); - mln_assertion(dk_old <= ek_old); + 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 << "reg/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. + mln_assertion(0 <= d_k); + mln_assertion(d_k <= e_k); + + // Disabled because of the following 'if' +// mln_assertion(e_k <= d_k_old); +// mln_assertion(d_k_old <= e_k_old); + + if (d_k > d_k_old) + { + qR = qR_old; + qT = qT_old; + break; + } // Backing up results. - dk_old = dk; - ek_old = ek; + 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-5); + || norm::l2((qT - qT_old)) + norm::l2((qR - qR_old).to_vec()) > 1e-3); + + trace::exiting("registration::icp"); + return std::make_pair(qR, qT); + } + + + + /// Single call to ICP with all sites. + template <typename P, typename F> + inline + composed< translation<P::dim,float>,rotation<P::dim,float> > + icp(const p_array<P>& P_, + const p_array<P>& X, + const F& closest_point) + { + util::timer t; + t.start(); + + std::pair<algebra::quat,mln_vec(P)> pair = icp(P_, X, closest_point, + algebra::quat(1,0,0,0), + literal::zero); + std::cout << "icp = " << t << std::endl; typedef rotation<3u,float> rot_t; - rot_t tqR(qR); + rot_t tqR(pair.first); typedef translation<3u,float> trans_t; - trans_t tqT(qT); - composed<rot_t,trans_t> result(tqR, tqT); + trans_t tqT(pair.second); + composed<trans_t, rot_t> result(tqT, tqR); - trace::exiting("registration::icp"); return result; } + + /// Shuffle the sites in P_. + /// Use one third of P_'s sites for each run. + /// For each run, it removes sites which are too close or too far. template <typename P, typename F> inline - composed< rotation<P::dim,float>,translation<P::dim,float> > - icp(const p_array<P>& data_P, - const p_array<P>& model_X, - const F& closest_point) + composed< translation<P::dim,float>,rotation<P::dim,float> > + icp_clean(const p_array<P>& P_, + const p_array<P>& X, + const F& closest_point) { - return icp(data_P, model_X, closest_point, - algebra::quat(1,0,0,0), literal::zero); + util::timer t; + t.start(); + + // P_bak is shuffled. + p_array<P> P_bak = P_; + std::vector<mln_element(p_array<P>)>& v = P_bak.hook_std_vector_(); + std::random_shuffle(v.begin(), v.end()); + + // P_sub = 1/3 * P_bak; + p_array<P> P_sub = P_bak; + P_sub.hook_std_vector_().resize(P_bak.nsites() / 3); + + unsigned r = 0; + 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(40); + image3d<value::rgb8> out(box); + p_array<P> removed_set; + do + { + /// Compute transformation. + pair = icp(P_sub, X, closest_point, + pair.first, + pair.second); + + pair = icp(P_sub, X, closest_point, + pair.first, + pair.second); + + P_sub = remove_too_far_sites(out, P_sub, + closest_point, pair, X, removed_set, r); + + + ++r; + + //Add more data + if (r < 3) + for (unsigned i = (P_bak.nsites() / 3) * r; + i < (P_bak.nsites() / 3) * (r + 1); ++i) + { + P_sub.append(P_bak[i]); + } + + } while (r < 4); + std::cout << "icp = " << t << std::endl; + + typedef rotation<3u,float> rot_t; + rot_t tqR(pair.first); + typedef translation<3u,float> trans_t; + trans_t tqT(pair.second); + composed<trans_t,rot_t> result(tqT, tqR); + + return result; + } + + + /// Shuffle sites in P_. + /// Do the first run with all sites. + /// For each run, remove sites which are too far or too close. + template <typename P, typename F> + inline + composed< translation<P::dim,float>,rotation<P::dim,float> > + icp_clean2(const p_array<P>& P_, + const p_array<P>& X, + const F& closest_point) + { + util::timer t; + t.start(); + + // P_bak is shuffled. + p_array<P> P_bak = P_; + + unsigned r = 0; + 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(40); + image3d<value::rgb8> out(box); + p_array<P> removed_set; + do + { + pair = icp(P_bak, X, closest_point, + pair.first, + pair.second); + + P_bak = remove_too_far_sites(out, P_bak, + closest_point, pair, X, removed_set, r); + + ++r; + + } while (r < 4); + std::cout << "icp = " << t << std::endl; + + draw_last_run(box, P_bak, removed_set, X, pair.first, pair.second); + + typedef rotation<3u,float> rot_t; + rot_t tqR(pair.first); + typedef translation<3u,float> trans_t; + trans_t tqT(pair.second); + composed<trans_t,rot_t> result(tqT, tqR); + + return result; + } + + + + /// Run icp once with 1/10 of the sites and run it once again with the + /// resulting tranformation and all the sites. + template <typename P, typename F> + inline + composed< translation<P::dim,float>,rotation<P::dim,float> > + icp_fast(const p_array<P>& P_, + const p_array<P>& X, + const F& closest_point) + { + typedef std::pair<algebra::quat,mln_vec(P)> pair_t; + + p_array<P> P_sub = P_; + std::vector<mln_element(p_array<P>)>& v = P_sub.hook_std_vector_(); + std::random_shuffle(v.begin(), v.end()); + v.resize(P_.nsites() / 10); + + util::timer t; + t.start(); + pair_t tmp = icp(P_sub, X, closest_point, + algebra::quat(1,0,0,0), literal::zero); + + std::cout << "icp_1 - " << t << "s" << std::endl; + t.restart(); + + pair_t tmp2 = icp(P_, X, closest_point, + tmp.first, tmp.second); + + std::cout << "icp_2 - " << t << "s" << std::endl; + + typedef rotation<3u,float> rot_t; + rot_t tqR(tmp2.first); + typedef translation<3u,float> trans_t; + trans_t tqT(tmp2.second); + composed<rot_t,trans_t> result(tqR, tqT); + + return result; } # endif // ! MLN_INCLUDE_ONLY -- 1.5.6.5