Index: ChangeLog =================================================================== --- ChangeLog (revision 1842) +++ ChangeLog (working copy) @@ -1,3 +1,14 @@ +2008-04-05 Ugo Jardonnet <jardonnet@lrde.epita.fr> + + Sandbox: ICP: Remove intermediate p_array Xk. + + * sandbox/jardonnet/test/icp.cc: Move a big part of icp.hh here. + * sandbox/jardonnet/test/Makefile (icp++): Add rule. + * sandbox/jardonnet/registration/quat7.hh: Add direct access to the map. + * sandbox/jardonnet/registration/icp.hh: Remove Xk. + * sandbox/jardonnet/registration/tools.hh: Improve lazy_image interface + and checks. + 2008-04-05 Ugo Jardonnet <ugo.jardonnet@lrde.epita.fr> Sandbox: ICP: Add file for refactoring. Index: sandbox/jardonnet/test/icp.cc =================================================================== --- sandbox/jardonnet/test/icp.cc (revision 1842) +++ sandbox/jardonnet/test/icp.cc (working copy) @@ -4,17 +4,64 @@ #include <mln/io/pbm/save.hh> #include <sandbox/jardonnet/registration/icp.hh> +#include <sandbox/jardonnet/registration/tools.hh> -int main(int, char* argv[]) +void usage(char *argv[]) { + std::cout << "usage : " << argv[0] + << " cloud.pbm surface.pbm" << std::endl; + exit(1); +} + + +int main(int argc, char* argv[]) +{ + if (argc != 3) + usage(argv); + using namespace mln; + typedef image3d< bool > I3d; image2d< bool > img1; image2d< bool > img2; - + + //load images io::pbm::load(img1, argv[1]); io::pbm::load(img2, argv[2]); + + //convert to image3d + I3d cloud = convert::to_image3d(img1); + I3d surface = convert::to_image3d(img2); - io::pbm::save(registration::icp(img1,img2), "./+registred.pbm"); + //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); + + //Make a lazy_image map via function closest_point + closest_point<mln_point_(I3d)> fun(x, box_<point3d>(1000,1000,1)); + lazy_image< closest_point<mln_point_(I3d)> > map(fun); + + registration::icp(c, map); + +#ifndef NDEBUG + std::cout << "closest_point(Ck[i]) = " << fun.i << std::endl; + std::cout << "Pts processed = " << registration::pts << std::endl; +#endif + + // //init output image + // //FIXME: Should be + // //mln_concrete(I) output(res.bbox()) = convert::to_image<I>(res)? + // const box_<point2d> box2d(1000,1000,0); + // image2d<bool> output(box2d, 1); + + // //to 2d : projection (FIXME:if 3d) + // for (size_t i = 0; i < res.npoints(); i++) + // { + // point2d p(res[i][0], res[i][1]); + // if (output.has(p)) + // output(p) = true; + // } + + //io::pbm::save("./+registred.pbm"); } Index: sandbox/jardonnet/test/Makefile =================================================================== --- sandbox/jardonnet/test/Makefile (revision 1842) +++ sandbox/jardonnet/test/Makefile (working copy) @@ -16,8 +16,11 @@ run: time ./+sub.exe . . ; time ./+gsub.exe . . + icp: g++ icp.cc -I../../.. -g -o '+icp.exe' +icp++: + g++ icp.cc -I../../.. -O3 -DNDEBUG -o '+icp.exe' icp_max++: g++ icp_max.cc -I../../.. -O3 -DNDEBUG -pg -o '+icp_map.exe' Index: sandbox/jardonnet/registration/quat7.hh =================================================================== --- sandbox/jardonnet/registration/quat7.hh (revision 1842) +++ sandbox/jardonnet/registration/quat7.hh (working copy) @@ -59,10 +59,7 @@ template <typename P> void apply_on(const p_array<P>& input, p_array<P>& output) const { - assert(input.npoints() == output.npoints()); - - std::cout << _qR << std::endl; - + assert(input.npoints() == output.npoints()); assert(_qR.is_unit()); for (size_t i = 0; i < input.npoints(); i++) @@ -92,21 +89,28 @@ } - template <typename P> + template <typename P, typename M> quat7<P::dim> match(const p_array<P>& C, const algebra::vec<P::dim,float>& mu_C, - const p_array<P>& Xk, - const algebra::vec<P::dim,float>& mu_Xk) + const p_array<P>& Ck, + M& map) { - assert(C.npoints() == Xk.npoints()); + //mu_Xk = center map(Ck) + algebra::vec<P::dim,float> mu_Xk(literal::zero); + for (size_t i = 0; i < Ck.npoints(); ++i) + { + algebra::vec<P::dim,float> xki = map(Ck[i]); + mu_Xk += xki; + } + mu_Xk /= Ck.npoints(); + // qR - algebra::mat<P::dim,P::dim,float> Mk(literal::zero); for (size_t i = 0; i < C.npoints(); ++i) { - algebra::vec<P::dim,float> Ci = C[i]; - algebra::vec<P::dim,float> Xki = Xk[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(); Index: sandbox/jardonnet/registration/icp_subsampled.hh =================================================================== --- sandbox/jardonnet/registration/icp_subsampled.hh (revision 1842) +++ sandbox/jardonnet/registration/icp_subsampled.hh (working copy) @@ -149,7 +149,7 @@ p_array<mln_point(I3d)> c = convert::to_p_array(cloud); p_array<mln_point(I3d)> x = convert::to_p_array(surface); - //init qk + //init rigid transform qk quat7<3> qk; //create subsampled p_array Index: sandbox/jardonnet/registration/icp.hh =================================================================== --- sandbox/jardonnet/registration/icp.hh (revision 1842) +++ sandbox/jardonnet/registration/icp.hh (working copy) @@ -51,6 +51,10 @@ namespace registration { +#ifndef NDEBUG + static unsigned pts = 0; +#endif + /*! Registration FIXME : doxy * * @@ -69,45 +73,52 @@ template <typename P, typename M> inline p_array<P> - icp_(p_array<P>& C, - const p_array<P>&, - M& map) + icp_(const p_array<P>& C, + M& map, + quat7<P::dim>& qk) { trace::entering("registration::impl::icp_"); +#ifndef NDEBUG + //display registred points + std::cout << "Register " + << C.npoints() << " points" << std::endl; + std::cout << "k\terror\tdqk" << std::endl; +#endif + unsigned int k; - quat7<P::dim> old_qk, qk; + 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; - + const float epsilon = 1;//1e-3; //// step 1 k = 0; do { - - //// step 2 - //projection::fill_Xk(Ck, map, Xk); - //projection::de_base(Ck, X, Xk, err_bis); - projection::memo(Ck, Xk, map); - - mu_Xk = center(Xk); - - //// step 3 + //compute qk old_qk = qk; - qk = match(C, mu_C, Xk, mu_Xk); + qk = match(C, mu_C, Ck, map); - //// step 4 - qk.apply_on(C, Ck); // Ck+1 = qk(C) + //Ck+1 = qk(C) + qk.apply_on(C, Ck); - //// err = d(Ck+1,Xk) + //err = d(Ck+1,Xk) err = rms(Ck, Xk); - std::cout << k << ' ' << err << ' ' << (qk - old_qk).sqr_norm() << std::endl; //plot file +#ifndef NDEBUG + //plot file + std::cout << k << '\t' << err << '\t' + << (qk - old_qk).sqr_norm() << '\t' + << std::endl; + //count the number of points processed + pts += Ck.npoints(); +#endif + ++k; } while (k < 3 || (qk - old_qk).sqr_norm() > epsilon); @@ -117,50 +128,28 @@ } // end of namespace mln::registration::impl - - //Only for 2d and 3d image - template <typename I, typename J> + + // Only for 3d images + template <typename P, typename M> inline - mln_concrete(I) //FIXME: should return something else ? qk ? - icp(const Image<I>& cloud_, - const Image<J>& surface_) + quat7<P::dim> + icp(const p_array<P>& cloud, + M& map) { trace::entering("registration::icp"); - mln_precondition(exact(cloud_).has_data()); - mln_precondition(exact(surface_).has_data()); + + mln_precondition(cloud.npoints() != 0); + mln_precondition(P::dim == 3); - //convert to image: time consuming - typedef image3d<mln_value(I)> I3d; - I3d cloud = convert::to_image_3d(exact(cloud_)); - const I3d surface = convert::to_image_3d(exact(surface_)); - - //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); - - //build closest point map - //lazy_map<I3d> map(enlarge(bigger(c.bbox(),x.bbox()),50)); - //lazy_map<I3d> map(1000,1000,50); - - c_point<mln_point(I3d)> fun(x); - //Make via function - lazy_image< c_point<mln_point(I3d)> > map(fun); + //init rigid transform qk + quat7<P::dim> qk; - p_array<mln_point(I3d)> res = impl::icp_(c, x, map); + //run icp + p_array<P> res = impl::icp_(cloud, map, qk); - //to 2d : projection (FIXME:if 3d) - //mln_concrete(I) output = convert::to_image<I>(res)? - mln_concrete(I) output(exact(cloud_).domain()); - for (size_t i = 0; i < res.npoints(); i++) - { - point2d p(res[i][0], res[i][1]); - //FIXME: not necessary if output(res.bbox()) - if (output.has(p)) - output(p) = true; - } - trace::exiting("registration::icp"); - return output; + + return qk; } # endif // ! MLN_INCLUDE_ONLY Index: sandbox/jardonnet/registration/tools.hh =================================================================== --- sandbox/jardonnet/registration/tools.hh (revision 1842) +++ sandbox/jardonnet/registration/tools.hh (working copy) @@ -11,19 +11,24 @@ { template <typename P> - struct c_point + struct closest_point { typedef P input; typedef P result; - c_point(const p_array<P>& X) - : X(X) + closest_point(const p_array<P>& X, const box_<P>& box) + : X(X), box(box), i(0) { } result //inline operator () (const P& Ck) { + +#ifndef NDEBUG + ++i; +#endif + algebra::vec<P::dim,float> Cki = Ck; algebra::vec<P::dim,float> best_x = X[0]; float best_d = norm::l2(Cki - best_x); @@ -40,12 +45,14 @@ return algebra::to_point<P>(best_x); } - const box_<P> domain() + const box_<P>& domain() { - return X.bbox(); + return box; } const p_array<P>& X; + const box_<P> box; + mutable unsigned i; }; @@ -56,29 +63,32 @@ { // Fun is potentially an image. lazy_image(F& fun) - : value(fun.domain()), is_known(fun.domain()), functor(fun) + : value(fun.domain()), is_known(fun.domain()), fun(fun) { } - // FIXME: remove this constructor - lazy_image(const F& fun, int nrows, int ncols) - : value(nrows, ncols), is_known(nrows,ncols), functor(fun) + // FIXME: hack, remove this constructor + lazy_image(F& fun, int nrows, int ncols, int nslis) + : value(nrows, ncols,1), is_known(nrows,ncols,1), fun(fun) { } const mln_result(F) //inline operator() (const typename F::input& p) { + mln_precondition(fun.domain().has(p)); + //FIXME: What about domain? if (is_known(p)) return value(p); - value(p) = functor(p); + value(p) = fun(p); + is_known(p) = true; return value(p); } //FIXME: 3d -> //mln_dim(ml_input(input)) image3d<mln_result(F)> value; image3d<bool> is_known; - F& functor; + F& fun; }; @@ -165,26 +175,27 @@ } } */ - + + //const return a const template <typename T> inline const image3d<T>& - to_image_3d(const image3d<T>& img) + to_image3d(const image3d<T>& img) { return img; } - + //non const return non const template <typename T> image3d<T>& - to_image_3d(image3d<T>& img) + to_image3d(image3d<T>& img) { return img; } - + template <typename T> inline image3d<T> - to_image_3d(const image2d<T>& img) + to_image3d(const image2d<T>& img) { point3d pmin(img.domain().pmin()[0], img.domain().pmin()[1], 0); point3d pmax(img.domain().pmax()[0], img.domain().pmax()[1], 0);