[Olena-patches] 1834: Sandbox: ICP: Add icp_subsampled.

https://svn.lrde.epita.fr/svn/oln/trunk/milena Index: ChangeLog from Ugo Jardonnet <ugo.jardonnet@lrde.epita.fr> Sandbox: ICP: Add icp_subsampled. * sandbox/jardonnet/test/bench: New info file. * sandbox/jardonnet/test/icp_check.sh: Update. * sandbox/jardonnet/test/Makefile: Add rules icp_sub. * sandbox/jardonnet/test/icp_subsampled.cc: New icp_sub. * sandbox/jardonnet/registration/icp_subsampled.hh: New. Registers, at first, a subset of the cloud. * sandbox/jardonnet/TODO: Update. * sandbox/jardonnet/registration/quat7.hh: Fix vec initialization. * sandbox/jardonnet/registration/cloud.hh: Fix vec initialization. svn wrapper unexpectedly failed to send the the news. => Index: ChangeLog =================================================================== --- ChangeLog (revision 1833) +++ ChangeLog (working copy) @@ -1,3 +1,19 @@ +2008-04-03 Ugo Jardonnet <jardonnet@lrde.epita.fr> + + Sandbox: ICP: Add icp_subsampled. + + * sandbox/jardonnet/test/bench: New info file. + + * sandbox/jardonnet/test/icp_check.sh: Update. + * sandbox/jardonnet/test/Makefile: Add rules icp_sub. + * sandbox/jardonnet/test/icp_subsampled.cc: New icp_sub. + * sandbox/jardonnet/registration/icp_subsampled.hh: New. Registers, at + first, a subset of the cloud. + + * sandbox/jardonnet/TODO: Update. + * sandbox/jardonnet/registration/quat7.hh: Fix vec initialisation. + * sandbox/jardonnet/registration/cloud.hh: Fix vec initialisation. + 2008-04-02 Ugo Jardonnet <ugo.jardonnet@lrde.epita.fr> Sandbox: ICP: Fix quaternion computation. Index: sandbox/jardonnet/test/bench =================================================================== --- sandbox/jardonnet/test/bench (revision 0) +++ sandbox/jardonnet/test/bench (revision 1834) @@ -0,0 +1,32 @@ + +icp_0 +icp_lazy_0 + 0m27.662s 0m8.157s + + + +icp_0D +icp_lazy_0D + 0m7.496s 0m4.960s + + + +icp_0Df +icp_lazy_0Df + 0m8.973s 0m5.568s + + + +icp_0f +icp_lazy_0f + 0m28.490s 0m8.273s + + + +icp_3 +icp_lazy_3 + 0m5.800s 0m1.256s + + + +icp_3D +icp_lazy_3D + 0m0.752s 0m0.316s + + + +icp_3df +icp_lazy_3df + 0m0.780s 0m0.532s + + + +icp_3f +icp_lazy_3f + 0m6.468s 0m1.212s + + Index: sandbox/jardonnet/test/icp_subsampled.cc =================================================================== --- sandbox/jardonnet/test/icp_subsampled.cc (revision 0) +++ sandbox/jardonnet/test/icp_subsampled.cc (revision 1834) @@ -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_subsampled.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/test/icp_check.sh =================================================================== --- sandbox/jardonnet/test/icp_check.sh (revision 1833) +++ sandbox/jardonnet/test/icp_check.sh (working copy) @@ -3,6 +3,6 @@ for i in `\ls bin/` do echo execute $i 01.pbm 02.pbm - ./bin/$i 01.pbm 02.pbm > ./bin/log_$i + time ./bin/$i 01.pbm 02.pbm > ./bin/log_$i echo ./bin/log_$i done Index: sandbox/jardonnet/test/Makefile =================================================================== --- sandbox/jardonnet/test/Makefile (revision 1833) +++ sandbox/jardonnet/test/Makefile (working copy) @@ -13,18 +13,23 @@ gau: g++ gaussian.cc $(FLAG) -o '+gau.exe' +run: + time ./+sub.exe . . ; time ./+gsub.exe . . + + + icp: - g++ icp.cc -I../../.. -O1 -DNDEBUG -g -o '+icp.exe' + g++ icp.cc -I../../.. -g -o '+icp_map.exe' icp++: - g++ icp.cc -I../../.. -O3 -DNDEBUG -o '+icp.exe' + g++ icp.cc -I../../.. -O3 -DNDEBUG -o '+icp_map.exe' +icp_sub: + g++ icp_subsampled.cc -I../../.. -O3 -DNDEBUG -g -o '+icp_sub.exe' + icpsafe: g++ icp.cc -fsignaling-nans -ffloat-store -I../../.. -O1 -o '+icp.exe' -run: - time ./+sub.exe . . ; time ./+gsub.exe . . - plot: icp ./+icp.exe 01.pbm 02.pbm > log gnuplot plotscript @@ -46,8 +51,9 @@ 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: - rm -- ./bin/* + rm -f -- ./bin/* .PHONY: check \ No newline at end of file Index: sandbox/jardonnet/TODO =================================================================== --- sandbox/jardonnet/TODO (revision 1833) +++ sandbox/jardonnet/TODO (working copy) @@ -5,18 +5,9 @@ gaussian.cc: In function 'int main(int, char*)': gaussian.cc:22: error: no match for 'operator==' in 'img == out' - - +Check gaussian -adapt test for threshold (old thresholding) - - -- Enlever static dans projection::memo - -Note: -+01.bmp: 943 pts -30 itérations : 30*943 = 28290 calcul de ppp -Domaine +02.bmp: 777x480 = 362082 -nb de ppp calculé memo = 11749 - -map 1:40 -de_base 10.5 -memo 9.5 \ No newline at end of file +adapt test for threshold (old thresholding) +- - Index: sandbox/jardonnet/registration/icp_subsampled.hh =================================================================== --- sandbox/jardonnet/registration/icp_subsampled.hh (revision 0) +++ sandbox/jardonnet/registration/icp_subsampled.hh (revision 1834) @@ -0,0 +1,188 @@ +// 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>&, + M& map, + quat7<P::dim>& qk) + { + trace::entering("registration::impl::icp_"); + + quat7<P::dim> old_qk; + float err; + //float err_bis; + + p_array<P> Ck(C), Xk(C); //FIXME: Xk sould not copy C + + algebra::vec<P::dim,float> mu_C = center(C), mu_Xk; + + const float epsilon = 1; //e-3; + + qk.apply_on(C, Ck); + + unsigned int k = 0; + do { + + //project Ck over Xk + projection::fill_Xk(Ck, map, Xk); + + mu_Xk = center(Xk); + + //compute qk + old_qk = qk; + qk = match(C, mu_C, Xk, mu_Xk); + + //Ck+1 = qk(C) + qk.apply_on(C, Ck); + + //err = d(Ck+1,Xk) + err = rms(Ck, Xk); + + //generate plot file + std::cout << k << ' ' << err << ' ' << (qk - old_qk).sqr_norm() << std::endl; + + ++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_)); + + + //create a pair (distance map, closest point) + float w[27] = {1.4142, 1, 1.4142, 1.4142, 1, 1.4142, 0, 0, 0, + 1, 1, 1, 1, 1, 0, 0, 0, 0, + 1.4142, 1, 1.4142, 0, 0, 0, 0, 0, 0}; + w_window<mln_dpoint(I3d), float> chamfer = make::w_window3d(w); + std::pair<mln_ch_value(I3d,float), mln_ch_value(I3d,mln_point(I3d)) > map = + dt::chamfer(surface, chamfer); + + + //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); + + //init qk + quat7<3> qk; + + //create subsampled p_array + p_array<mln_point(I3d)> cprime; + for (size_t i = 0; i < c.npoints(); i += 9) //FIXME: randomize + cprime.append(c[i]); + + //apply icp 1 + impl::icp_(cprime, x, map, qk); + + //apply icp 2 + p_array<mln_point(I3d)> res = impl::icp_(c, x, 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; + } + +# endif // ! MLN_INCLUDE_ONLY + + } // end of namespace mln::registration + +} // end of namespace mln + + +#endif // ! MLN_REGISTRATION_ICP_HH Index: sandbox/jardonnet/registration/quat7.hh =================================================================== --- sandbox/jardonnet/registration/quat7.hh (revision 1833) +++ sandbox/jardonnet/registration/quat7.hh (working copy) @@ -29,6 +29,7 @@ algebra::vec<n,float> _qT; quat7() + : _qR(1,0,0,0), _qT(literal::zero) { } @@ -59,6 +60,9 @@ void apply_on(const p_array<P>& input, p_array<P>& output) const { assert(input.npoints() == output.npoints()); + + std::cout << _qR << std::endl; + assert(_qR.is_unit()); for (size_t i = 0; i < input.npoints(); i++) Index: sandbox/jardonnet/registration/cloud.hh =================================================================== --- sandbox/jardonnet/registration/cloud.hh (revision 1833) +++ sandbox/jardonnet/registration/cloud.hh (working copy) @@ -20,7 +20,7 @@ template <typename P> P center(const p_array<P>& a) { - algebra::vec<P::dim,float> c; + algebra::vec<P::dim,float> c(literal::zero); for (size_t i = 0; i < a.npoints(); ++i) { algebra::vec<P::dim,float> ai = a[i]; Index: sandbox/jardonnet/registration/jacobi.hh =================================================================== --- sandbox/jardonnet/registration/jacobi.hh (revision 1833) +++ sandbox/jardonnet/registration/jacobi.hh (working copy) @@ -20,11 +20,12 @@ void jacobi(algebra::mat<4,4,float> a, algebra::quat& q) { float dd, d[4]; - algebra::mat<4,4,float> v; + algebra::mat<4,4,float> v(literal::zero); int j,iq,ip,i = 0; float tresh,theta,tau,t,sm,s,h,g,c,b[4],z[4]; for (ip=0;ip<4;ip++) { - for (iq=0;iq<4;iq++) v(ip,iq)=0.0; + for (iq=0;iq<4;iq++) + v(ip,iq)=0.0; v(ip,ip)=1.0; } for (ip=0;ip<4;ip++) { Index: sandbox/jardonnet/registration/rotation.hh =================================================================== --- sandbox/jardonnet/registration/rotation.hh (revision 1833) +++ sandbox/jardonnet/registration/rotation.hh (working copy) @@ -23,7 +23,6 @@ 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(); }