https://svn.lrde.epita.fr/svn/oln/trunk/milena Index: ChangeLog from Ugo Jardonnet <jardonnet@lrde.epita.fr> Sandbox: ICP: Make check. * sandbox/jardonnet/test/icp_check.sh: New Check Script. * sandbox/jardonnet/test/bin: New Bin directory. * sandbox/jardonnet/test/Makefile: Update for testes. * sandbox/jardonnet/test/icp_lazy.cc: New dedicated to lazy_map. * sandbox/jardonnet/registration/icp_lazy.hh: New dedicated to lazy map. * sandbox/jardonnet/registration/quat7.hh: Update. * sandbox/jardonnet/registration/chamfer.hh: Update. * sandbox/jardonnet/registration/icp.hh: Update (dedicated to distance_map). * sandbox/jardonnet/registration/misc.hh: Move outside quat/ (removed). * sandbox/jardonnet/registration/rotation.hh: Move outside quat/ (removed). * sandbox/jardonnet/test/check: Remove old test script. registration/icp.hh | 6 + registration/icp_lazy.hh | 171 +++++++++++++++++++++++++++++++++++++++++++++++ registration/misc.hh | 19 +++++ registration/quat7.hh | 2 registration/rotation.hh | 70 +++++++++++++++++++ test/Makefile | 25 ++++++ test/icp_check.sh | 8 ++ test/icp_lazy.cc | 20 +++++ 8 files changed, 315 insertions(+), 6 deletions(-) Index: sandbox/jardonnet/test/icp_check.sh --- sandbox/jardonnet/test/icp_check.sh (revision 0) +++ sandbox/jardonnet/test/icp_check.sh (revision 0) @@ -0,0 +1,8 @@ +#!/bin/sh + +for i in `\ls bin/` +do + echo execute $i 01.pbm 02.pbm + ./bin/$i 01.pbm 02.pbm > ./bin/log_$i + echo ./bin/log_$i +done Property changes on: sandbox/jardonnet/test/icp_check.sh ___________________________________________________________________ Name: svn:executable + * Index: sandbox/jardonnet/test/Makefile --- sandbox/jardonnet/test/Makefile (revision 1820) +++ sandbox/jardonnet/test/Makefile (working copy) @@ -14,9 +14,6 @@ g++ gaussian.cc $(FLAG) -o '+gau.exe' icp: - g++ icp.cc $(FLAG) -o '+icp.exe' - -icp++: g++ icp.cc -ffloat-store -I../../.. -O3 -DNDEBUG -o '+icp.exe' icpsafe: @@ -29,3 +26,25 @@ ./+icp.exe 01.pbm 02.pbm > log gnuplot plotscript +check: clean + g++ icp.cc -I../../.. -O0 -o './bin/+icp_0' + g++ icp.cc -I../../.. -O0 -ffloat-store -o './bin/+icp_0f' + g++ icp.cc -I../../.. -O0 -DNDEBUG -o './bin/+icp_0D' + g++ icp.cc -I../../.. -O0 -DNDEBUG -ffloat-store -o './bin/+icp_0Df' + g++ icp.cc -I../../.. -O3 -o './bin/+icp_3' + 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' + +clean: + rm -- ./bin/* + +.PHONY: check \ No newline at end of file Index: sandbox/jardonnet/test/icp_lazy.cc --- sandbox/jardonnet/test/icp_lazy.cc (revision 0) +++ sandbox/jardonnet/test/icp_lazy.cc (revision 0) @@ -0,0 +1,20 @@ +#include <mln/core/image3d.hh> + +#include <mln/io/pbm/load.hh> +#include <mln/io/pbm/save.hh> + +#include <sandbox/jardonnet/registration/icp_lazy.hh> + +int main(int, char* argv[]) +{ + using namespace mln; + + image2d< bool > img1; + image2d< bool > img2; + + io::pbm::load(img1, argv[1]); + io::pbm::load(img2, argv[2]); + + io::pbm::save(registration::icp(img1,img2), "./+registred.pbm"); +} + Index: sandbox/jardonnet/registration/quat7.hh --- sandbox/jardonnet/registration/quat7.hh (revision 1820) +++ sandbox/jardonnet/registration/quat7.hh (working copy) @@ -8,7 +8,7 @@ # include <mln/algebra/mat.hh> # include <mln/core/p_array.hh> -# include "quat/all.hh" +# include "rotation.hh" # include "jacobi.hh" # include <mln/norm/l2.hh> Index: sandbox/jardonnet/registration/chamfer.hh Index: sandbox/jardonnet/registration/icp_lazy.hh --- sandbox/jardonnet/registration/icp_lazy.hh (revision 0) +++ sandbox/jardonnet/registration/icp_lazy.hh (revision 0) @@ -0,0 +1,171 @@ +// Copyright (C) 2008 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_REGISTRATION_ICP_HH +# define MLN_REGISTRATION_ICP_HH + +/*! \file mln/registration/icp.hh + * + * \brief image registration + */ + +# include <mln/algebra/quat.hh> +# include <mln/algebra/vec.hh> +# include <mln/make/w_window.hh> +# include <mln/make/w_window3d.hh> + +# include "tools.hh" + +# include "cloud.hh" +# include "quat7.hh" +# include "projection.hh" +# include "chamfer.hh" + +namespace mln +{ + + namespace registration + { + + /*! Registration FIXME : doxy + * + * + */ + template <typename I, typename J> + inline + mln_concrete(I) + icp(const Image<I>& cloud, + const Image<J>& surface); + +# ifndef MLN_INCLUDE_ONLY + + namespace impl + { + + template <typename P, typename M> + inline + p_array<P> + icp_(p_array<P>& C, + const p_array<P>& X, + M& map) + { + trace::entering("registration::impl::icp_"); + + unsigned int k; + quat7<P::dim> old_qk, 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 { + //std::cout << "step 2" << std::endl; + + //// step 2 + projection::memo(Ck, X, Xk, map); + + mu_Xk = center(Xk); + + //std::cout << "step 3" << std::endl; + //// step 3 + old_qk = qk; + qk = match(C, mu_C, Xk, mu_Xk); + + //std::cout << "step 4" << std::endl; + //// step 4 + qk.apply_on(C, Ck); // Ck+1 = qk(C) + + //std::cout << "step err" << std::endl; + //// err = d(Ck+1,Xk) + err = rms(Ck, Xk); + std::cout << k << ' ' << err << ' ' << (qk - old_qk).sqr_norm() << std::endl; //plot file + + ++k; + } while (k < 3 || (qk - old_qk).sqr_norm() > epsilon); + + trace::exiting("registration::impl::icp_"); + return Ck; + } + + } // end of namespace mln::registration::impl + + + //Only for 2d and 3d image + template <typename I, typename J> + inline + mln_concrete(I) //FIXME: should return something else ? qk ? + icp(const Image<I>& cloud_, + const Image<J>& surface_) + { + trace::entering("registration::icp"); + mln_precondition(exact(cloud_).has_data()); + mln_precondition(exact(surface_).has_data()); + + //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 + //FIXME: like that lazy_map<I3d> map(enlarge(bigger(c.bbox(),x.bbox()),50)); + lazy_map<I3d> map(1000,1000,50); + + p_array<mln_point(I3d)> res = impl::icp_(c, x, map); + + //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; + } + +# endif // ! MLN_INCLUDE_ONLY + + } // end of namespace mln::registration + +} // end of namespace mln + + +#endif // ! MLN_REGISTRATION_ICP_HH Index: sandbox/jardonnet/registration/icp.hh --- sandbox/jardonnet/registration/icp.hh (revision 1820) +++ sandbox/jardonnet/registration/icp.hh (working copy) @@ -87,8 +87,10 @@ //// step 1 k = 0; + do { - std::cout << "step 2" << std::endl; + //std::cout << "step 2" << std::endl; + //// step 2 projection::fill_Xk(Ck, map, Xk); //projection::de_base(Ck, X, Xk, err_bis); @@ -157,7 +159,7 @@ p_array<mln_point(I3d)> res = impl::icp_(c, x, map); //to 2d : projection (FIXME:if 3d) - //mln_concrete(I) output = convert::to_image2d(res)? + //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++) { Index: sandbox/jardonnet/registration/misc.hh --- sandbox/jardonnet/registration/misc.hh (revision 0) +++ sandbox/jardonnet/registration/misc.hh (revision 0) @@ -0,0 +1,19 @@ +#ifndef MISC_HH +# define MISC_HH + + +inline +float epsilon() +{ + static const float e = 1e-5; + return e; +} + +inline +bool about_equal(float val1, float val2) +{ + return fabs(val1 - val2) < epsilon(); +} + + +#endif // ndef MISC_HH Index: sandbox/jardonnet/registration/rotation.hh --- sandbox/jardonnet/registration/rotation.hh (revision 0) +++ sandbox/jardonnet/registration/rotation.hh (revision 0) @@ -0,0 +1,70 @@ +#ifndef ROTATION_HH +# define ROTATION_HH + +# include <stdlib.h> +//# include "quat.hh" + +//# include "mat.hh" + +# include <mln/algebra/mat.hh> +# include <mln/algebra/vec.hh> +# include <mln/make/vec.hh> +# include <mln/make/mat.hh> +# include <mln/algebra/quat.hh> + +# include "misc.hh" + +// FIXME: rotation should be an abstract class +// and derived classes encapsulate either a quaternion or a algebra::matrix +namespace mln +{ + + // FIXME : quat is not appriate here + template <unsigned n> + algebra::vec<n,float> rotate(const algebra::quat& q, const algebra::vec<n,float>& p) + { + + return (q * algebra::quat(0. ,p) * q.inv()).v(); + } + + //FIXME : check if correct with n != 3 + template <unsigned n> + bool check_rotation(const algebra::mat<n,n,float>& mat, + const algebra::quat& q) + { + assert(q.is_unit()); + algebra::vec<n,float> + tmp = make::vec(rand(), rand(), rand()), + 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); + } + + + //FIXME : switch to n dim. + algebra::mat<3,3,float> quat2mat(const algebra::quat& q) + { + assert(q.is_unit()); + float + w = q.to_vec()[0], + x = q.to_vec()[1], x2 = 2*x*x, xw = 2*x*w, + y = q.to_vec()[2], y2 = 2*y*y, xy = 2*x*y, yw = 2*y*w, + z = q.to_vec()[3], z2 = 2*z*z, xz = 2*x*z, yz = 2*y*z, zw = 2*z*w; + float t[9] = {1.f - y2 - z2, xy - zw, xz + yw, + xy + zw, 1.f - x2 - z2, yz - xw, + xz - yw, yz + xw, 1.f - x2 - y2}; + algebra::mat<3,3,float> tmp = make::mat<3,3,9,float>(t); + // postcondition + assert(check_rotation(tmp, q)); + return tmp; + } +} + + +#endif // ndef ROTATION_HH