[Olena-patches] 1810: Sandbox: ICP: draft improvment.

https://svn.lrde.epita.fr/svn/oln/trunk/milena Index: ChangeLog from Ugo Jardonnet &lt;ugo.jardonnet(a)lrde.epita.fr&gt; Sandbox: ICP: draft improvment. * sandbox/jardonnet/test/Makefile: -O3 version. * sandbox/jardonnet/test/icp.cc: image loading. * sandbox/jardonnet/registration/quat7.hh: Update. * sandbox/jardonnet/registration/chamfer.hh: Etienne's. * sandbox/jardonnet/registration/cloud.hh: Correction. * sandbox/jardonnet/registration/icp.hh: Update. * sandbox/jardonnet/registration/projection.hh: To be removed. * sandbox/jardonnet/registration/quat/rotation.hh: s/3/n/. mln/make/mat.hh | 2 sandbox/folio/naive.cc | 14 + sandbox/jardonnet/registration/chamfer.hh | 189 ++++++++++++++++++++++++ sandbox/jardonnet/registration/cloud.hh | 7 sandbox/jardonnet/registration/icp.hh | 124 +++++++++++++-- sandbox/jardonnet/registration/projection.hh | 3 sandbox/jardonnet/registration/quat/rotation.hh | 7 sandbox/jardonnet/registration/quat7.hh | 5 sandbox/jardonnet/test/Makefile | 3 sandbox/jardonnet/test/icp.cc | 16 +- 10 files changed, 334 insertions(+), 36 deletions(-) Index: mln/make/mat.hh --- mln/make/mat.hh (revision 1809) +++ mln/make/mat.hh (working copy) @@ -71,7 +71,7 @@ { algebra::mat<n,1,T> tmp; for (unsigned i = 0; i < n; i++) - tmp(i,1) = v[i]; + tmp(i,0) = v[i]; return tmp; } Index: sandbox/jardonnet/test/icp.cc --- sandbox/jardonnet/test/icp.cc (revision 1809) +++ sandbox/jardonnet/test/icp.cc (working copy) @@ -1,16 +1,22 @@ #include <mln/core/image3d.hh> -#include <mln/io/ppm/load.hh> -#include <mln/io/ppm/save.hh> +#include <mln/io/pbm/load.hh> +#include <mln/io/pbm/save.hh> #include <sandbox/jardonnet/registration/icp.hh> -int main(int, char*) +int main(int, char* argv[]) { using namespace mln; - image3d< value::rgb8 > img; + image2d< bool > img1; + image2d< bool > img2; - registration::icp(img,img); + io::pbm::load(img1, argv[1]); + io::pbm::load(img2, argv[2]); + + image2d< bool > res(img1.domain()); + + io::pbm::save(registration::icp(img1,img2,res), "./+registred.pbm"); } Index: sandbox/jardonnet/test/Makefile --- sandbox/jardonnet/test/Makefile (revision 1809) +++ sandbox/jardonnet/test/Makefile (working copy) @@ -16,6 +16,9 @@ icp: g++ icp.cc $(FLAG) -o '+icp.exe' +icp++: + g++ icp.cc -I../../.. -O3 -o '+icp.exe' + run: time ./+sub.exe . . ; time ./+gsub.exe . . Index: sandbox/jardonnet/registration/quat7.hh --- sandbox/jardonnet/registration/quat7.hh (revision 1809) +++ sandbox/jardonnet/registration/quat7.hh (working copy) @@ -122,20 +122,23 @@ // qR //FIXME : use P::dim ? + std::cout << "loop" << std::endl; algebra::mat<P::dim,P::dim,float> Mk; for (unsigned i = 0; i < C.npoints(); ++i) { algebra::vec<P::dim,float> Ci = C[i]; - algebra::vec<P::dim,float> Xki = Xki; + algebra::vec<P::dim,float> Xki = Xk[i]; Mk += make::mat(Ci - mu_C) * trans(make::mat(Xki - mu_Xk)); } Mk /= C.npoints(); + std::cout << "loop end" << std::endl; const algebra::mat<P::dim,P::dim,float> Ak = Mk - trans(Mk); const float v[3] = {Ak(1,2), Ak(2,0), Ak(0,1)}; const algebra::mat<3,1,float> D = make::mat<3,1,3,float>(v); // FIXME why <...> + algebra::mat<4,4,float> Qk; Qk(0,0) = tr(Mk); put(trans(D), 0,1, Qk); Index: sandbox/jardonnet/registration/chamfer.hh --- sandbox/jardonnet/registration/chamfer.hh (revision 0) +++ sandbox/jardonnet/registration/chamfer.hh (revision 0) @@ -0,0 +1,189 @@ +// Copyright (C) 2007 EPITA Research and Development Laboratory +// +// This file is part of the Olena Library. This library is free +// software; you can redistribute it and/or modify it under the terms +// of the GNU General Public License version 2 as published by the +// Free Software Foundation. +// +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this library; see the file COPYING. If not, write to +// the Free Software Foundation, 51 Franklin Street, Fifth Floor, +// Boston, MA 02111-1307, USA. +// +// As a special exception, you may use this file as part of a free +// software library without restriction. Specifically, if other files +// instantiate templates or use macros or inline functions from this +// file, or you compile this file and link it with other files to +// produce an executable, this file does not by itself cause the +// resulting executable to be covered by the GNU General Public +// License. This exception does not however invalidate any other +// reasons why the executable file might be covered by the GNU General +// Public License. + +#ifndef MLN_DT_CHAMFER_HH +# define MLN_DT_CHAMFER_HH + +# include <mln/core/concept/image.hh> +# include <mln/make/w_window.hh> + +namespace mln +{ + + namespace dt + { + + /*! Distance tranform by chamfer application. + * + * \param[in] input_ The input image. + * \param[in] chamfer The chamfer window to use for distance calcul. + * \return A pair (distance map, nearest point map). + * + * \pre \p img has to be initialized. + */ + template<typename I, typename T> + std::pair<mln_ch_value(I, T), mln_ch_value(I, mln_point(I))> + chamfer(const Image<I>& input_, + w_window<mln_dpoint(I), T> chamfer); + + +# ifndef MLN_INCLUDE_ONLY + + namespace impl + { + + /*! Computes a pass of the chamfer DT algorithm. + * + * \param[in] p Iterator on the input image to use. + * \param[in] chamfer The chamfer window to use for distance calcul. + * \param[in] input The input image. + * \param[out] outputDistance The distance map updated. + * \param[out] outputnearest The nearest points map updated. + */ + template<typename Q, typename I, typename T> + inline + void + chamfer_pass(const w_window<mln_dpoint(I), T> chamfer, + const I& input, + mln_ch_value(I, T)& outputDistance, + mln_ch_value(I, mln_point(I))& outputNearest) + { + typedef w_window<mln_dpoint(I), T> W; + + Q p(input.domain()); + mln_qiter(W) q(chamfer, p); + for_all(p) + { + std::pair<T, mln_point(I)> min(mln_max(T), p); + + for_all(q) + if (input.has(q) && outputDistance(q) != mln_max(T)) + { + T v = outputDistance(q) + q.w(); + + if (v < min.first) + { + min.first = v; + min.second = outputNearest(q); + } + } + + if (min.first < outputDistance(p)) + { + outputDistance(p) = min.first; + outputNearest(p) = min.second; + } + } + } + + } // end of namespace mln::dt::impl + + + + // Facade. + + template<typename I, typename T> + inline + std::pair<mln_ch_value(I, T), mln_ch_value(I, mln_point(I))> + chamfer(const Image<I>& input_, + w_window<mln_dpoint(I), T> chamfer) + { + typedef w_window<mln_dpoint(I), T> W; + + const I& input = exact(input_); + mln_precondition(input.has_data()); + + mln_ch_value(I, T) outputDistance; + initialize(outputDistance, input); + + mln_ch_value(I, mln_point(I)) outputNearest; + initialize(outputNearest, input); + + // Initialization. + { + mln_fwd_piter(I) p(input.domain()); + for_all(p) + { + outputDistance(p) = input(p) ? literal::zero : mln_max(T); + outputNearest(p) = p; + } + } + + // First pass. + impl::chamfer_pass<mln_fwd_piter(I)> + (chamfer, input, outputDistance, outputNearest); + + chamfer.sym(); + + // Second pass. + impl::chamfer_pass<mln_bkd_piter(I)> + (chamfer, input, outputDistance, outputNearest); + + return std::pair<mln_ch_value(I, T), mln_ch_value(I, mln_point(I))> + (outputDistance, outputNearest); + } + +# endif // ! MLN_INCLUDE_ONLY + + } // end of namespace mln::dt + +} // end of namespace mln + +#endif // ! MLN_DT_CHAMFER_HH + +// #include <iostream> +// #include <mln/debug/println.hh> +// #include <mln/core/image2d.hh> +// #include <mln/make/win_chamfer.hh> +// #include <mln/level/fill.hh> + +// int main() +// { +// using namespace mln; + +// w_window2d_int chamfer = make::mk_chamfer_3x3_int<3, 4>(); + +// { +// image2d<bool> ima(5,5); +// bool vals[] = { 1, 1, 1, 0, 0, +// 0, 0, 1, 0, 0, +// 0, 0, 1, 0, 0, +// 0, 0, 0, 0, 0, +// 0, 0, 0, 0, 0 }; + +// level::fill(ima, vals); +// debug::println(ima); + +// std::pair<image2d<int>, image2d<mln_point_(image2d<bool>)> > +// out = dt::chamfer(ima, chamfer); + +// std::cerr << "Distance:" << std::endl; +// debug::println(out.first); +// std::cerr << "PPP:" << std::endl; +// debug::println(out.second); +// } +// } Index: sandbox/jardonnet/registration/cloud.hh --- sandbox/jardonnet/registration/cloud.hh (revision 1809) +++ sandbox/jardonnet/registration/cloud.hh (working copy) @@ -32,9 +32,12 @@ // FIXME : move //exist for P? template <typename P> - float sqr_norm(const P& v) + mln_coord(P) sqr_norm(const P& v) { - return v[1] * v[1] + v[2] * v[2] + v[3] * v[3]; + mln_coord(P) tmp = 0; + for (unsigned i = 0; i < P::dim; i++) + tmp += v[i] * v[i]; + return tmp; } template <typename P> Index: sandbox/jardonnet/registration/icp.hh --- sandbox/jardonnet/registration/icp.hh (revision 1809) +++ sandbox/jardonnet/registration/icp.hh (working copy) @@ -35,14 +35,12 @@ # include <mln/algebra/quat.hh> # include <mln/algebra/vec.hh> - - -//typedef mln::algebra::vec<3, float> vec3f; -//typedef mln::p_array< vec3f > vecs_t; +# include <mln/core/w_window.hh> #include "cloud.hh" #include "quat7.hh" #include "projection.hh" +# include "chamfer.hh" namespace mln { @@ -56,20 +54,23 @@ */ template <typename I, typename J> inline - void + I icp(const Image<I>& cloud, - const Image<J>& surface); + const Image<J>& surface, + I& r); # ifndef MLN_INCLUDE_ONLY namespace impl { - template <typename P> + template <typename P, typename T1, typename T2> inline void icp_(const p_array<P>& C, - const p_array<P>& X) + const p_array<P>& X, + std::pair<T1,T2>&, + p_array<P>& Ck) { trace::entering("registration::impl::icp_"); @@ -77,9 +78,12 @@ quat7<P::dim> old_qk, qk; float err, err_bis; - p_array<P> Ck, Xk; - Ck.reserve(C.npoints()); - Xk.reserve(Ck.npoints()); + assert(Ck.npoints() == C.npoints()); + p_array<P> Xk(C); //FIXME:is it correct? + /// Ck.reserve(C.npoints()); + //Xk.reserve(C.npoints()); + //assert(C.npoints() != 0); + algebra::vec<P::dim,float> mu_C = center(C), mu_Xk; const float epsilon = 1e-3; @@ -89,15 +93,19 @@ Ck = C; do { + std::cout << "step2" << std::endl; //step 2 FIXME : etienne projection::de_base(Ck, X, Xk, err_bis); + std::cout << "step2.1 center" << std::endl; mu_Xk = center(Xk); + std::cout << "step3" << std::endl; // step 3 old_qk = qk; qk = match(C, mu_C, Xk, mu_Xk); + std::cout << "step4" << std::endl; // step 4 qk.apply_on(C, Ck); // Ck+1 = qk(C) @@ -105,6 +113,7 @@ err = rms(Ck, Xk); ++k; + std::cout << err << std::endl; } while (k < 3 || (qk - old_qk).sqr_norm() > epsilon); trace::exiting("registration::impl::icp_"); @@ -113,20 +122,97 @@ } // end of namespace mln::registration::impl - // this version could convert image cloud in a vector of point? + //FIXME: move? + namespace convert + { + template <typename T> + const image3d<T>& + to_image_3d(const image3d<T>& img) + { + return img; + } + + template <typename T> + image3d<T>& + to_image_3d(image3d<T>& img) + { + return img; + } + + template <typename T> + image3d<T> + to_image_3d(const image2d<T>& img) + { + point3d pmin(img.domain().pmin()[0], img.domain().pmin()[1], -10); + point3d pmax(img.domain().pmax()[0], img.domain().pmax()[1], 10); + image3d<T> img3d(box3d(pmin,pmax)); + + mln_piter(image3d<T>) p(img3d.domain()); + for_all(p) + { + if (p[2] == 0) + img3d(p) = exact(img)(point2d(p[0],p[1])); + } + return img3d; + } + } + + + //Only for 2d and 3d image template <typename I, typename J> inline - void - icp(const Image<I>& cloud, - const Image<J>& surface) + I + icp(Image<I>& cloud_, + const Image<J>& surface_, + I& r) { trace::entering("registration::icp"); - mln_precondition(exact(cloud).has_data()); - mln_precondition(exact(surface).has_data()); + mln_precondition(exact(cloud_).has_data()); + mln_precondition(exact(surface_).has_data()); - p_array<mln_point(I)> a,b; // FIXME : to built. + std::cout << "convert to image3d" << std::endl; + //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_)); - impl::icp_(a, b); + + std::cout << "chamfer" << std::endl; + /* + //FIXME: not a chamfer. etienne? + //create a pair (distance map, closest point) + w_window<mln_dpoint(I3d), float> chamfer; + */ + std::pair<mln_ch_value(I3d,float), mln_ch_value(I3d,mln_point(I3d)) > maps; + /* + dt::chamfer(surface, chamfer); + */ + + std::cout << "Build p_array" << std::endl; + //build p_arrays. + p_array<mln_point(I3d)> c,x; + + mln_piter(I3d) p1(cloud.domain()); + for_all(p1) + if (exact(cloud)(p1)) + c.append(p1); + + mln_piter(I3d) p2(surface.domain()); + for_all(p2) + if (exact(surface)(p2)) + x.append(p2); + + std::cout << "Start ICP" << std::endl; + + p_array<mln_point(I3d)> res(c); + impl::icp_(c, x, maps, res); + + //to 2d + for (unsigned e; e < res.npoints(); e++) + { + point2d p(res[e][0], res[e][1]); + r(p) = true; + } trace::exiting("registration::icp"); } Index: sandbox/jardonnet/registration/projection.hh --- sandbox/jardonnet/registration/projection.hh (revision 1809) +++ sandbox/jardonnet/registration/projection.hh (working copy) @@ -18,7 +18,7 @@ p_array<P>& Xk, float& err) { - assert(Ck.npoints() == Xk.npoints()); + // assert(Ck.npoints() == Xk.npoints()); err = 0.f; @@ -37,6 +37,7 @@ best_x = Xj; } } + if (i < Xk.npoints()) // FIXME:double hack Xk.hook_()[i] = algebra::to_point<P>(best_x); err += best_d; } Index: sandbox/jardonnet/registration/quat/rotation.hh --- sandbox/jardonnet/registration/quat/rotation.hh (revision 1809) +++ sandbox/jardonnet/registration/quat/rotation.hh (working copy) @@ -35,9 +35,14 @@ assert(q.is_unit()); algebra::vec<n,float> tmp = make::vec(rand(), rand(), rand()), - p = tmp / norm::l2(tmp), + p; + float nl2= norm::l2(tmp); + if(nl2 != 0) + p = tmp / nl2; + algebra::vec<n,float> p_rot_1 = rotate(q, p), p_rot_2 = mat * p; + return about_equal(norm::l2(p_rot_1 - p_rot_2), 0.f); } Index: sandbox/folio/naive.cc --- sandbox/folio/naive.cc (revision 1809) +++ sandbox/folio/naive.cc (working copy) @@ -54,8 +54,9 @@ * * \fixme Use instead of R the result type of F::operator(). */ - template <typename I, typename F, typename R> - mln_ch_value(I, R) + template <typename I, typename F> + inline + mln_ch_value(I, typename F::ret) naive(const Image<I>& input_, F& fun_); @@ -63,15 +64,15 @@ // Facade. - template <typename I, typename F, typename R> + template <typename I, typename F> inline - mln_ch_value(I, R) + mln_ch_value(I, typename F::ret) naive(const Image<I>& input_, F& fun_) { const I& input = exact(input_); mln_precondition(input.has_data()); - mln_ch_value(I, R) output; + mln_ch_value(I, typename F::ret) output; initialize(input.domain()); mln_piter(I) p(input.domain()); @@ -82,7 +83,7 @@ else { // p is in the background so the distance has to be computed. - accu::min_<R> min; + accu::min_<typename F::ret> min; min.init(); mln_piter(I) q(input.domain()); @@ -117,6 +118,7 @@ struct l2norm { + typedef float ret; template <typename C, typename D> D operator()(C a, C b)