[Olena-patches] 1854: Sandbox: ICP: Add multi-scale registration.

https://svn.lrde.epita.fr/svn/oln/trunk/milena Index: ChangeLog from Ugo Jardonnet <ugo.jardonnet@lrde.epita.fr> Sandbox: ICP: Add multi-scale registration. * sandbox/jardonnet/test/icp.cc: Add nb_it as argument for a registration. * sandbox/jardonnet/test/Makefile: Remove icp_lazy test. Add parameter for multi-scale icp: * sandbox/jardonnet/registration/tools.hh: Update. * sandbox/jardonnet/registration/quat7.hh: Use virtual length (c_length) for p_array. * sandbox/jardonnet/registration/cloud.hh: Use virtual length. * sandbox/jardonnet/registration/icp.hh: Use virtual length. Following files now exist eleswhere (registration/) * sandbox/jardonnet/registration/quat: Remove. * sandbox/jardonnet/registration/quat/all.hh: Remove. * sandbox/jardonnet/registration/quat/misc.hh: Remove. * sandbox/jardonnet/registration/quat/rotation.hh: Remove. * sandbox/jardonnet/registration/projection.hh: Use virtual length. registration/cloud.hh | 15 ++++++---- registration/icp.hh | 62 +++++++++++++++++++++++++++++++-------------- registration/projection.hh | 7 ++--- registration/quat7.hh | 24 ++++++++++------- registration/tools.hh | 26 +----------------- test/Makefile | 8 ----- test/icp.cc | 6 ++-- 7 files changed, 77 insertions(+), 71 deletions(-) Index: sandbox/jardonnet/test/icp.cc --- sandbox/jardonnet/test/icp.cc (revision 1852) +++ sandbox/jardonnet/test/icp.cc (working copy) @@ -41,18 +41,20 @@ closest_point<mln_point_(I3d)> fun(x, box_<point3d>(1000,1000,1)); lazy_image< closest_point<mln_point_(I3d)> > map(fun); - quat7<3> q = registration::icp(c, map); + quat7<3> q = registration::icp(c, map, 2); #ifndef NDEBUG std::cout << "closest_point(Ck[i]) = " << fun.i << std::endl; std::cout << "Pts processed = " << registration::pts << std::endl; #endif + //FIXME: register img1 + //init output image //FIXME: Should be //mln_concrete(I) output(res.bbox()) = convert::to_image<I>(res)? - q.apply_on(c, c); + q.apply_on(c, c, c.npoints()); const box_<point2d> box2d(1000,1000); image2d<bool> output(box2d, 1); Index: sandbox/jardonnet/test/Makefile --- sandbox/jardonnet/test/Makefile (revision 1852) +++ sandbox/jardonnet/test/Makefile (working copy) @@ -48,14 +48,6 @@ g++ icp.cc -I../../.. -O3 -ffloat-store -o './bin/+icp_3f' g++ icp.cc -I../../.. -O3 -DNDEBUG -o './bin/+icp_3D' g++ icp.cc -I../../.. -O3 -DNDEBUG -ffloat-store -o './bin/+icp_3Df' - g++ icp_lazy.cc -I../../.. -O0 -o './bin/+icp_lazy_0' - g++ icp_lazy.cc -I../../.. -O0 -ffloat-store -o './bin/+icp_lazy_0f' - g++ icp_lazy.cc -I../../.. -O0 -DNDEBUG -o './bin/+icp_lazy_0D' - g++ icp_lazy.cc -I../../.. -O0 -DNDEBUG -ffloat-store -o './bin/+icp_lazy_0Df' - g++ icp_lazy.cc -I../../.. -O3 -o './bin/+icp_lazy_3' - g++ icp_lazy.cc -I../../.. -O3 -ffloat-store -o './bin/+icp_lazy_3f' - g++ icp_lazy.cc -I../../.. -O3 -DNDEBUG -o './bin/+icp_lazy_3D' - g++ icp_lazy.cc -I../../.. -O3 -DNDEBUG -ffloat-store -o './bin/+icp_lazy_3Df' ./icp_check.sh clean: Index: sandbox/jardonnet/registration/tools.hh --- sandbox/jardonnet/registration/tools.hh (revision 1852) +++ sandbox/jardonnet/registration/tools.hh (working copy) @@ -10,6 +10,7 @@ namespace mln { + //FIXME: groe length template <typename P> struct closest_point { @@ -18,11 +19,9 @@ closest_point(const p_array<P>& X, const box_<P>& box) : X(X), box(box) - #ifndef NDEBUG , i(0) #endif - { } result @@ -80,6 +79,7 @@ : value(nrows, ncols,1), is_known(nrows,ncols,1), fun(fun) { } + // FIXME: gore length const mln_result(F) //inline operator () (const typename F::input& p) const @@ -254,28 +254,6 @@ } // end of namespace convert - - namespace fun - { - //FIXME: temporary - template <typename C, typename T= float> - struct cham : public Function_p2v< cham<C,T> > - { - typedef T result; - //bad - T operator()(dpoints_fwd_piter<mln::dpoint_<mln::grid::cube, int> >& v) const - { - C o = C::origin; - if (v < o) - return 1.; - else - return 0.; - } - }; - } // end of namespace fun - - - namespace algebra { Index: sandbox/jardonnet/registration/quat7.hh --- sandbox/jardonnet/registration/quat7.hh (revision 1852) +++ sandbox/jardonnet/registration/quat7.hh (working copy) @@ -59,12 +59,15 @@ } template <typename P> - void apply_on(const p_array<P>& input, p_array<P>& output) const + void apply_on(const p_array<P>& input, + p_array<P>& output, + const size_t c_length) const { - assert(input.npoints() == output.npoints()); + assert(c_length <= input.npoints()); + assert(c_length <= output.npoints()); assert(_qR.is_unit()); - for (size_t i = 0; i < input.npoints(); i++) + for (size_t i = 0; i < c_length; i++) output.hook_()[i] = algebra::to_point<P>((*this)(input[i])); } @@ -95,27 +98,30 @@ quat7<P::dim> match(const p_array<P>& C, const algebra::vec<P::dim,float>& mu_C, const p_array<P>& Ck, - M& map) + M& map, + size_t c_length) { + //FIXME: mix mu_Xk and qk loop + + //mu_Xk = center map(Ck) algebra::vec<P::dim,float> mu_Xk(literal::zero); - for (size_t i = 0; i < Ck.npoints(); ++i) + for (size_t i = 0; i < c_length; ++i) { algebra::vec<P::dim,float> xki = map(Ck[i]); mu_Xk += xki; } - mu_Xk /= Ck.npoints(); - + mu_Xk /= c_length; // qR algebra::mat<P::dim,P::dim,float> Mk(literal::zero); - for (size_t i = 0; i < C.npoints(); ++i) + for (size_t i = 0; i < c_length; ++i) { algebra::vec<P::dim,float> Ci = C[i]; algebra::vec<P::dim,float> Xki = map(Ck[i]); Mk += make::mat(Ci - mu_C) * trans(make::mat(Xki - mu_Xk)); } - Mk /= C.npoints(); + Mk /= c_length; /* Index: sandbox/jardonnet/registration/cloud.hh --- sandbox/jardonnet/registration/cloud.hh (revision 1852) +++ sandbox/jardonnet/registration/cloud.hh (working copy) @@ -18,16 +18,16 @@ { template <typename P> - P center(const p_array<P>& a) + P center(const p_array<P>& a, size_t length) { algebra::vec<P::dim,float> c(literal::zero); - for (size_t i = 0; i < a.npoints(); ++i) + for (size_t i = 0; i < length; ++i) { algebra::vec<P::dim,float> ai = a[i]; c += ai; } - return algebra::to_point<P>(c / a.npoints()); + return algebra::to_point<P>(c / length); } @@ -43,16 +43,19 @@ template <typename P> float rms(const p_array<P>& a1, - const p_array<P>& a2) + const p_array<P>& a2, + const size_t length) { - assert(a1.npoints() == a2.npoints()); + assert(length <= a1.npoints()); + assert(length <= a2.npoints()); /* float f = 0.f; for (size_t i = 0; i < a1.npoints(); ++i) f += sqr_norm(a1[i] - a2[i]); return f / a1.npoints();*/ + float f = 0.f; - for (size_t i = 0; i < a1.npoints(); ++i) + for (size_t i = 0; i < length; ++i) { algebra::vec<3,float> a1f = a1[i]; algebra::vec<3,float> a2f = a2[i]; Index: sandbox/jardonnet/registration/icp.hh --- sandbox/jardonnet/registration/icp.hh (revision 1852) +++ sandbox/jardonnet/registration/icp.hh (working copy) @@ -35,6 +35,7 @@ # include <iostream> # include <string> +# include <cmath> # include <mln/algebra/quat.hh> # include <mln/algebra/vec.hh> @@ -78,48 +79,56 @@ p_array<P> icp_(const p_array<P>& C, M& map, - quat7<P::dim>& qk) + quat7<P::dim>& qk, + const size_t c_length, + const float epsilon = 1e-3) { trace::entering("registration::impl::icp_"); #ifndef NDEBUG //display registred points std::cout << "Register " - << C.npoints() << " points" << std::endl; + << c_length << " points" << std::endl; std::cout << "k\terror\tdqk" << std::endl; #endif - - unsigned int k; quat7<P::dim> old_qk; float err; //float err_bis; p_array<P> Ck(C), Xk(C); //FIXME: Xk copy C - algebra::vec<P::dim,float> mu_C = center(C), mu_Xk; + algebra::vec<P::dim,float> mu_C = center(C, c_length), mu_Xk; - const float epsilon = 1;//1e-3; + qk.apply_on(C, Ck, c_length); - //// step 1 - k = 0; + unsigned int k = 0; do { //compute qk old_qk = qk; - qk = match(C, mu_C, Ck, map); + qk = match(C, mu_C, Ck, map, c_length); //Ck+1 = qk(C) - qk.apply_on(C, Ck); + qk.apply_on(C, Ck, c_length); //err = d(Ck+1,Xk) - err = rms(Ck, Xk); + err = rms(Ck, Xk, c_length); #ifndef NDEBUG - { using namespace std; - image2d<bool> img = convert::to_image2d(Ck); + + image2d<bool> img(box2d(500,800), 0); + for (size_t i = 0; i < c_length; i++) + { + point2d p = convert::to_point2d(Ck[i]); + if (img.has(p)) + img(p) = true; + } + + //image2d<bool> img = convert::to_image2d(Ck); stringstream oss; - oss << "reg" << k << ".pbm"; + static int pimp = 0; + oss << "i_" << pimp++ << ".pbm"; io::pbm::save(img, oss.str()); } @@ -128,10 +137,10 @@ << (qk - old_qk).sqr_norm() << '\t' << std::endl; //count the number of points processed - pts += Ck.npoints(); + pts += c_length; #endif - ++k; + k++; } while (k < 3 || (qk - old_qk).sqr_norm() > epsilon); trace::exiting("registration::impl::icp_"); @@ -145,19 +154,34 @@ template <typename P, typename M> inline quat7<P::dim> - icp(const p_array<P>& cloud, - M& map) + icp(p_array<P> cloud, + M& map, + const unsigned nb_it) { trace::entering("registration::icp"); mln_precondition(P::dim == 3); mln_precondition(cloud.npoints() != 0); + // Shuffle cloud + for (size_t i = 0; i < cloud.npoints(); i++) + { + size_t r = rand() % cloud.npoints(); + P tmp; + tmp = cloud[i]; + cloud.hook_()[i] = cloud[r]; + cloud.hook_()[r] = tmp; + } + //init rigid transform qk quat7<P::dim> qk; //run icp - p_array<P> res = impl::icp_(cloud, map, qk); + for (int i = nb_it; i >= 0; i--) + { + size_t l = cloud.npoints() / std::pow(2., i); // 3 is arbitrary fixed + impl::icp_(cloud, map, qk, l, i + 1e-3); + } trace::exiting("registration::icp"); Index: sandbox/jardonnet/registration/projection.hh --- sandbox/jardonnet/registration/projection.hh (revision 1852) +++ sandbox/jardonnet/registration/projection.hh (working copy) @@ -70,11 +70,12 @@ template <typename P, typename T> void memo(const p_array<P>& Ck, p_array<P>& Xk, - lazy_image<T>& map) // first: closest points, second: is_computed + lazy_image<T>& map, + const size_t c_length) // first: closest points, second: is_computed { - assert(Ck.npoints() == Xk.npoints()); + //assert(Ck.npoints() == Xk.npoints()); //FIXME: - for (size_t i = 0; i < Ck.npoints(); ++i) + for (size_t i = 0; i < c_length; ++i) Xk.hook_()[i] = map(Ck[i]); }