https://svn.lrde.epita.fr/svn/oln/trunk/milena/sandbox Index: ChangeLog from Ugo Jardonnet <jardonnet@lrde.epita.fr> Sandbox: ICP: Points escaping during multi-scale registration. * jardonnet/test/test.rb: Fix: Change parameters e and q. * jardonnet/test/icp.cc: Fix: Make use of update_qk[1]. * jardonnet/registration/icp_ref.hh: Fix: Remove useless Xk array. * jardonnet/registration/icp.hh: Update: Escape points during registration steps. registration/icp.hh | 16 +++++++++++++++- registration/icp_ref.hh | 2 +- test/icp.cc | 4 +++- test/test.rb | 4 ++-- 4 files changed, 21 insertions(+), 5 deletions(-) Index: jardonnet/test/test.rb --- jardonnet/test/test.rb (revision 2003) +++ jardonnet/test/test.rb (working copy) @@ -3,8 +3,8 @@ print "# icp_ref \t icp\n" print "-------------------------\n" -q = 1 -e = 1 +q = 4 +e = 5 for i in 0..9 do Index: jardonnet/test/icp.cc --- jardonnet/test/icp.cc (revision 2003) +++ jardonnet/test/icp.cc (working copy) @@ -79,8 +79,9 @@ for (size_t i = 0; i < c.npoints(); i++) length[i] = norm::l2(algebra::vec<3,int> (c[i] - map(c[i]))); + // final transform - quat7<3> fqk = registration::final_qk2(c, map, 2*stddev); + quat7<3> fqk = registration::final_qk(c, map, 2*stddev); fqk.apply_on(c, c, c.npoints()); //print x @@ -92,6 +93,7 @@ output(px) = literal::green; } + //to 2d : projection (FIXME:if 3d) for (size_t i = 0; i < c.npoints(); i++) { Index: jardonnet/registration/icp_ref.hh --- jardonnet/registration/icp_ref.hh (revision 2003) +++ jardonnet/registration/icp_ref.hh (working copy) @@ -102,7 +102,7 @@ buffer<4,quat7<P::dim> > buf_qk; - p_array<P> Ck(C), Xk(C); + p_array<P> Ck(C); float d_k = 1000, d_k_1 = 1000, e_k = 1000; algebra::vec<P::dim,float> mu_C = center(C, c_length), mu_Xk; Index: jardonnet/registration/icp.hh --- jardonnet/registration/icp.hh (revision 2003) +++ jardonnet/registration/icp.hh (working copy) @@ -151,7 +151,7 @@ pts += c_length; #endif k++; - } while (/*k < 3 ||*/ (qk - buf_qk[1]).sqr_norm() / qk.sqr_norm() > epsilon); + } while ((qk - buf_qk[1]).sqr_norm() / qk.sqr_norm() > epsilon); trace::exiting("registration::impl::icp_"); } @@ -203,6 +203,20 @@ l = (l<1) ? 1 : l; impl::icp_(cloud, map, qk, l, 1e-3); + //remove points + p_array<P> tmp; + tmp.reserve(cloud.npoints()); + qk.apply_on(cloud,tmp, cloud.npoints()); + float stddev, mean; + mean_stddev(tmp, map, mean, stddev); + for (unsigned i = 0; i < cloud.npoints(); i++) + { + algebra::vec<3,float> qci = tmp[i]; + algebra::vec<3,float> xi = x[i]; + if (norm::l2(qci - xi) < 2 * stddev) + cloud.hook_()[i] = cloud[(i+1) % cloud.npoints()]; + } + #ifndef NDEBUG { value::rgb8 c;