https://svn.lrde.epita.fr/svn/oln/trunk/milena/sandbox Index: ChangeLog from Ugo Jardonnet <jardonnet@lrde.epita.fr> Sandbox: ICP: Make icp stop if e_k < d_k (errors). * jardonnet/test/icp_ref.cc: Fix p_array initialisation. * jardonnet/registration/icp_ref.hh (impl::icp) : Update : Stop if e_k < d_k. registration/icp_ref.hh | 11 ++++++----- registration/tools.hh | 1 + test/icp_ref.cc | 33 ++++++++++++++++++++++++++------- 3 files changed, 33 insertions(+), 12 deletions(-) Index: jardonnet/test/icp_ref.cc --- jardonnet/test/icp_ref.cc (revision 2000) +++ jardonnet/test/icp_ref.cc (working copy) @@ -22,9 +22,8 @@ if (argc != 5) usage(argv); - float q = std::atof(argv[3]); - int e = std::atoi(argv[4]); - + float q = 1;//std::atof(argv[3]); + int e = 1;//std::atoi(argv[4]); if (q < 1 or e < 1) usage(argv); @@ -44,14 +43,34 @@ I3d surface = convert::to_image3d(img2); //build p_arrays. - p_array<mln_point_(I3d)> c = convert::to_p_array(cloud); - p_array<mln_point_(I3d)> x = convert::to_p_array(surface); + p_array< point_<grid::cube, float> > c; + { + mln_piter_(I3d) p(cloud.domain()); + for_all(p) + if (cloud(p)) + { + point_<grid::cube,float> p_; + p_[0] = p[0]; p_[1] = p[1]; p_[2] = p[2]; + c.append(p_); + } + } + p_array< point_<grid::cube, float> > x; + { + mln_piter_(I3d) p(surface.domain()); + for_all(p) + if (surface(p)) + { + point_<grid::cube,float> p_; + p_[0] = p[0]; p_[1] = p[1]; p_[2] = p[2]; + x.append(p_); + } + } //working box - const box_<mln_point_(I3d)> working_box = enlarge(bigger(c.bbox(),x.bbox()),100); + const box_< point_<grid::cube, float> > working_box = enlarge(bigger(c.bbox(),x.bbox()),100); // FIXME : TODO : map : vec<3,float> -> point - closest_point<mln_point_(I3d)> map(x, working_box); + closest_point< point_<grid::cube, float> > map(x, working_box); quat7<3> qk = registration::icp(c, map, q, e, x); Index: jardonnet/registration/tools.hh --- jardonnet/registration/tools.hh (revision 2000) +++ jardonnet/registration/tools.hh (working copy) @@ -200,6 +200,7 @@ return a; } + template < typename P > inline box_<point2d> Index: jardonnet/registration/icp_ref.hh --- jardonnet/registration/icp_ref.hh (revision 2000) +++ jardonnet/registration/icp_ref.hh (working copy) @@ -103,7 +103,7 @@ buffer<4,quat7<P::dim> > buf_qk; p_array<P> Ck(C), Xk(C); - float d_k = 1000, d_k_1 = 1000; + float d_k = 1000, d_k_1 = 1000, e_k = 1000; algebra::vec<P::dim,float> mu_C = center(C, c_length), mu_Xk; @@ -127,10 +127,10 @@ // = d(closest(qk-1(P)), qk(P)) d_k = rms(C, map, c_length, buf_qk[1], qk); -#ifndef NDEBUG // e_k = d(closest(qk-1(P)), qk-1(P)) - float e_k = rms(C, map, c_length, buf_qk[1], buf_qk[1]); + e_k = rms(C, map, c_length, buf_qk[1], buf_qk[1]); +#ifndef NDEBUG //plot file std::cout << k << '\t' << (e_k >= d_k ? ' ' : '-') << '\t' << e_k << '\t' << d_k << '\t' << ((qk - buf_qk[1]).sqr_norm() / qk.sqr_norm()) << '\t' @@ -139,7 +139,8 @@ pts += c_length; #endif k++; - } while (d_k_1 - d_k > epsilon); + } while (e_k >= d_k && d_k_1 - d_k > epsilon); + // FIXME : test (e_k >= d_k) seems to be a bad idea. trace::exiting("registration::impl::icp_"); } @@ -200,7 +201,7 @@ mln_piter(p_array<P>) p(cloud); for_all(p) { - algebra::vec<3,float> p_ = point3d(p), qp_ = qk(p_); + algebra::vec<3,float> p_ = P(p), qp_ = qk(p_); point2d qp = make::point2d(int(qp_[0]), int(qp_[1])); if (tmp.has(qp)) tmp(qp) = c;