[Olena-patches] 1792: Icp : n dim, refactored, uses p_array, vec -> rounded point proc.

https://svn.lrde.epita.fr/svn/oln/trunk/milena Index: ChangeLog from Ugo Jardonnet &lt;ugo.jardonnet(a)lrde.epita.fr&gt; Icp : n dim, refactored, uses p_array, vec -> rounded point proc. * mln/core/point.hh: Fix convertion to vec. * mln/algebra/quat.hh: Fix namespace. * mln/algebra/vec.hh: add to_point procedure. * sandbox/jardonnet/test/icp.cc, * sandbox/jardonnet/registration/quat7.hh, * sandbox/jardonnet/registration/cloud.hh, * sandbox/jardonnet/registration/jacobi.hh, * sandbox/jardonnet/registration/icp.hh, * sandbox/jardonnet/registration/projection.hh, * sandbox/jardonnet/registration/quat/all.hh, * sandbox/jardonnet/registration/quat/rotation.hh: Update for p_array and n dim (not ready yet). * sandbox/jardonnet/registration/quat/interpol.hh: Remove. Provided by qut.hh mln/algebra/quat.hh | 24 ++++----- mln/algebra/vec.hh | 12 ++++ sandbox/jardonnet/registration/cloud.hh | 37 ++++++++------ sandbox/jardonnet/registration/icp.hh | 35 ++++++++------ sandbox/jardonnet/registration/jacobi.hh | 10 ++-- sandbox/jardonnet/registration/projection.hh | 33 ++++++------- sandbox/jardonnet/registration/quat/all.hh | 1 sandbox/jardonnet/registration/quat/rotation.hh | 21 +++++--- sandbox/jardonnet/registration/quat7.hh | 60 ++++++++++++++---------- sandbox/jardonnet/test/icp.cc | 4 - 10 files changed, 136 insertions(+), 101 deletions(-) Index: mln/core/point.hh Index: mln/algebra/quat.hh --- mln/algebra/quat.hh (revision 1791) +++ mln/algebra/quat.hh (working copy) @@ -30,7 +30,7 @@ /*! \file mln/algebra/quat.hh * - * \brief Define a class for quaternion values. + * \brief Define a class for quaternion algebra values. */ # include <cmath> @@ -44,12 +44,13 @@ # include <mln/algebra/vec.hh> # include <mln/norm/l2.hh> +//FIXME: pow, exp etc... are def here and in value::... namespace mln { // Fwd decls. - namespace value { class quat; } + namespace algebra { class quat; } namespace literal { struct zero_t; struct one_t; } @@ -114,16 +115,15 @@ - namespace value + namespace algebra { - // FIXME doesn't compile - + // FIXME value::Vectorial ??? value ??? class quat : - public Vectorial< quat > + public value::Vectorial< quat > , - public internal::value_like_< algebra::vec<4, float>, // Equivalent. + public value::internal::value_like_< algebra::vec<4, float>, // Equivalent. algebra::vec<4, float>, // Encoding. algebra::vec<4, float>, // Interoperation. quat > // Exact. @@ -606,7 +606,7 @@ quat slerp_2(const quat& p, const quat& q, float h) { assert(interpol_ok(p, q, h)); - quat tmp = p * value::pow(p.conj() * q, h); + quat tmp = p * pow(p.conj() * q, h); assert(about_equal(tmp, slerp(p, q, h))); return tmp; } @@ -615,7 +615,7 @@ quat slerp_3(const quat& p, const quat& q, float h) { assert(interpol_ok(p, q, h)); - quat tmp = value::pow(p * q.conj(), 1 - h) * q; + quat tmp = pow(p * q.conj(), 1 - h) * q; assert(about_equal(tmp, slerp(p, q, h))); return tmp; } @@ -624,7 +624,7 @@ quat slerp_4(const quat& p, const quat& q, float h) { assert(interpol_ok(p, q, h)); - quat tmp = value::pow(q * p.conj(), h) * p; + quat tmp = pow(q * p.conj(), h) * p; assert(about_equal(tmp, slerp(p, q, h))); return tmp; } @@ -633,7 +633,7 @@ quat slerp_5(const quat& p, const quat& q, float h) { assert(interpol_ok(p, q, h)); - quat tmp = q * value::pow(q.conj() * p, 1 - h); + quat tmp = q * pow(q.conj() * p, 1 - h); assert(about_equal(tmp, slerp(p, q, h))); return tmp; } @@ -641,7 +641,7 @@ # endif // ! MLN_INCLUDE_ONLY - } // end of namespace mln::value + } // end of namespace mln::algebra } // end of namespace mln Index: mln/algebra/vec.hh --- mln/algebra/vec.hh (revision 1791) +++ mln/algebra/vec.hh (working copy) @@ -549,6 +549,18 @@ return tmp; } + + template <typename P, unsigned n> + inline + P + to_point(const vec<n,float>& v) + { + P tmp; + for (unsigned i = 0; i < P::dim; ++i) + tmp[i] = round(v[i]); + return tmp; + } + # endif // MLN_INCLUDE_ONLY } // end of namespace mln::algebra Index: sandbox/jardonnet/test/icp.cc --- sandbox/jardonnet/test/icp.cc (revision 1791) +++ sandbox/jardonnet/test/icp.cc (working copy) @@ -1,4 +1,4 @@ -#include <mln/core/image2d.hh> +#include <mln/core/image3d.hh> #include <mln/io/ppm/load.hh> #include <mln/io/ppm/save.hh> @@ -9,7 +9,7 @@ { using namespace mln; - image2d< value::rgb8 > img; + image3d< value::rgb8 > img; registration::icp(img,img); } Index: sandbox/jardonnet/registration/quat7.hh --- sandbox/jardonnet/registration/quat7.hh (revision 1791) +++ sandbox/jardonnet/registration/quat7.hh (working copy) @@ -6,6 +6,7 @@ # include <algorithm> # include <mln/algebra/mat.hh> +# include <mln/core/p_array.hh> # include "quat/all.hh" # include "jacobi.hh" @@ -49,16 +50,17 @@ namespace mln { + template <unsigned n> struct quat7 { - value::quat _qR; - vec3f _qT; + algebra::quat _qR; + algebra::vec<n,float> _qT; quat7() { } - quat7(const value::quat& qR, const vec3f& qT) : + quat7(const algebra::quat& qR, const algebra::vec<n,float>& qT) : _qR(qR), _qT(qT) { @@ -76,19 +78,21 @@ // quat7 is an object-function - vec3f operator()(const vec3f& v) const + algebra::vec<n,float> operator()(const algebra::vec<n,float>& v) const { return rotate(_qR, v) + _qT; } - void apply_on(const std::vector< vec3f >& input, std::vector< vec3f >& output) const + template <typename P> + void apply_on(const p_array<P>& input, p_array<P>& output) const { - assert(input.size() == output.size()); + assert(input.npoints() == output.npoints()); assert(_qR.is_unit()); - std::transform(input.begin(), input.end(), - output.begin(), - *this); + //FIXME utiliser equivalent pour p_array + //std::transform(input.begin(), input.end(), + // output.begin(), + // *this); } }; @@ -107,42 +111,48 @@ } - quat7 match(const vecs_t& P, - const vec3f& mu_P, - const vecs_t& Xk, - const vec3f& mu_Xk) + template <typename P> + 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) { - assert(P.size() == Xk.size()); + assert(C.npoints() == Xk.npoints()); // qR - algebra::mat<3,3,float> Ck; - for (unsigned i = 0; i < P.size(); ++i) - Ck += make::mat(P[i] - mu_P) * trans(make::mat(Xk[i] - mu_Xk)); - Ck /= P.size(); + //FIXME : use P::dim ? + 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; + Mk += make::mat(Ci - mu_C) * trans(make::mat(Xki - mu_Xk)); + } + Mk /= C.npoints(); - const algebra::mat<3,3,float> Ak = Ck - trans(Ck); + 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(Ck); + Qk(0,0) = tr(Mk); put(trans(D), 0,1, Qk); put(D, 1,0, Qk); - put(Ck + trans(Ck) - algebra::mat<3,3,float>::identity() * tr(Ck), 1,1, Qk); + put(Mk + trans(Mk) - algebra::mat<P::dim,P::dim,float>::identity() * tr(Mk), 1,1, Qk); - value::quat qR; + algebra::quat qR; jacobi(Qk, qR); // qT - const vec3f qT = mu_Xk - rotate(qR, mu_P); + const algebra::vec<P::dim,float> qT = mu_Xk - rotate(qR, mu_C); - return quat7(qR, qT); + return quat7<P::dim>(qR, qT); } -} +} //end of namespace mln #endif // ndef QUAT7_HH Index: sandbox/jardonnet/registration/cloud.hh --- sandbox/jardonnet/registration/cloud.hh (revision 1791) +++ sandbox/jardonnet/registration/cloud.hh (working copy) @@ -8,6 +8,7 @@ # include <sstream> # include <mln/algebra/vec.hh> +# include <mln/core/p_array.hh> namespace mln { @@ -15,32 +16,36 @@ namespace registration { - typedef algebra::vec<3, float> vec3f; - - - vec3f center(const std::vector< vec3f >& vecs) + template <typename P> + P center(const p_array<P>& a) + { + algebra::vec<P::dim,float> c; + for (size_t i = 0; i < a.npoints(); ++i) { - vec3f c; - for (size_t i = 0; i < vecs.size(); ++i) - c += vecs[i]; - return c / vecs.size(); + algebra::vec<P::dim,float> ai = a[i]; + c += ai; + } + + return algebra::to_point<P>(c / a.npoints()); } - // FIXME : move - float sqr_norm(const vec3f& v) + // FIXME : move //exist for P? + template <typename P> + float sqr_norm(const P& v) { return v[1] * v[1] + v[2] * v[2] + v[3] * v[3]; } - float rms(const std::vector< vec3f >& vecs1, - const std::vector< vec3f >& vecs2) + template <typename P> + float rms(const p_array<P>& a1, + const p_array<P>& a2) { - assert(vecs1.size() == vecs2.size()); + assert(a1.npoints() == a2.npoints()); float f = 0.f; - for (size_t i = 0; i < vecs1.size(); ++i) - f += sqr_norm(vecs1[i] - vecs2[i]); - return f / vecs1.size(); + for (size_t i = 0; i < a1.npoints(); ++i) + f += sqr_norm(a1[i] - a2[i]); + return f / a1.npoints(); } } Index: sandbox/jardonnet/registration/jacobi.hh --- sandbox/jardonnet/registration/jacobi.hh (revision 1791) +++ sandbox/jardonnet/registration/jacobi.hh (working copy) @@ -7,14 +7,16 @@ // from num. rec. in C +namespace mln +{ #define rotateJacobi(a,i,j,k,l) g=a(i,j);h=a(k,l);a(i,j)=g-s*(h+g*tau); \ a(k,l)=h+s*(g-h*tau); -void jacobi(mln::algebra::mat<4,4,float> a, mln::value::quat& q) + void jacobi(algebra::mat<4,4,float> a, algebra::quat& q) { float dd, d[4]; - mln::algebra::mat<4,4,float> v; + algebra::mat<4,4,float> v; 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++) { @@ -39,7 +41,7 @@ iq = ip; dd = d[ip]; } - q = mln::value::quat(v(0,iq), + q = algebra::quat(v(0,iq), v(1,iq), v(2,iq), v(3,iq)); @@ -103,6 +105,6 @@ } } - +} #endif // ndef JACOBI_HH Index: sandbox/jardonnet/registration/icp.hh --- sandbox/jardonnet/registration/icp.hh (revision 1791) +++ sandbox/jardonnet/registration/icp.hh (working copy) @@ -33,12 +33,12 @@ * \brief image registration */ -# include <mln/value/quat.hh> +# include <mln/algebra/quat.hh> # include <mln/algebra/vec.hh> -typedef mln::algebra::vec<3, float> vec3f; -typedef std::vector< vec3f > vecs_t; +//typedef mln::algebra::vec<3, float> vec3f; +//typedef mln::p_array< vec3f > vecs_t; #include "cloud.hh" #include "quat7.hh" @@ -65,39 +65,44 @@ namespace impl { + template <typename P> inline void - icp_(const vecs_t& P, - const vecs_t& X) + icp_(const p_array<P>& C, + const p_array<P>& X) { trace::entering("registration::impl::icp_"); unsigned int k; - quat7 old_qk, qk; + quat7<P::dim> old_qk, qk; float err, err_bis; - vecs_t Pk(P.size()), Xk(Pk.size()); - vec3f mu_P = center(P), mu_Xk; + p_array<P> Ck, Xk; + Ck.reserve(C.npoints()); + Xk.reserve(Ck.npoints()); + algebra::vec<P::dim,float> mu_C = center(C), mu_Xk; const float epsilon = 1e-3; //step 1 k = 0; - Pk = P; + Ck = C; do { //step 2 FIXME : etienne - projection::de_base(Pk, X, Xk, mu_Xk, err_bis); + projection::de_base(Ck, X, Xk, err_bis); + + mu_Xk = center(Xk); // step 3 old_qk = qk; - qk = match(P, mu_P, Xk, mu_Xk); + qk = match(C, mu_C, Xk, mu_Xk); // step 4 - qk.apply_on(P, Pk); // Pk+1 = qk(P) + qk.apply_on(C, Ck); // Ck+1 = qk(C) - // err = d(Pk+1,Xk) - err = rms(Pk, Xk); + // err = d(Ck+1,Xk) + err = rms(Ck, Xk); ++k; } while (k < 3 || (qk - old_qk).sqr_norm() > epsilon); @@ -119,7 +124,7 @@ mln_precondition(exact(cloud).has_data()); mln_precondition(exact(surface).has_data()); - vecs_t a,b; // FIXME : to built. + p_array<mln_point(I)> a,b; // FIXME : to built. impl::icp_(a, b); Index: sandbox/jardonnet/registration/projection.hh --- sandbox/jardonnet/registration/projection.hh (revision 1791) +++ sandbox/jardonnet/registration/projection.hh (working copy) @@ -10,40 +10,37 @@ namespace projection { - template <class Pk_t, class X_t, class Xk_t> + template <typename P> void de_base(// input - const Pk_t& Pk, - const X_t& X, + const p_array<P>& Ck, + const p_array<P>& X, // inout - Xk_t& Xk, - // output - algebra::vec<3, float>& mu_Xk, + p_array<P>& Xk, float& err) { - assert(Pk.size() == Xk.size()); + assert(Ck.npoints() == Xk.npoints()); err = 0.f; - mu_Xk = make::vec(0,0,0); - for (size_t i = 0; i < Pk.size(); ++i) + for (size_t i = 0; i < Ck.npoints(); ++i) { - algebra::vec<3,float> best_x = X[0]; - float best_d = norm::l2(Pk[i] - best_x); - for (size_t j = 1; j < X.size(); ++j) + algebra::vec<P::dim,float> Cki = Ck[i]; + algebra::vec<P::dim,float> best_x = X[0]; + float best_d = norm::l2(Cki - best_x); + for (size_t j = 1; j < X.npoints(); ++j) { - float d = norm::l2(Pk[i] - X[j]); + algebra::vec<P::dim,float> Xj = X[j]; + float d = norm::l2(Cki - Xj); if (d < best_d) { best_d = d; - best_x = X[j]; + best_x = Xj; } } - Xk[i] = best_x; - mu_Xk += Xk[i]; + Xk.hook_()[i] = algebra::to_point<P>(best_x); err += best_d; } - mu_Xk /= Pk.size(); - err /= Pk.size(); + err /= Ck.npoints(); } } Index: sandbox/jardonnet/registration/quat/all.hh --- sandbox/jardonnet/registration/quat/all.hh (revision 1791) +++ sandbox/jardonnet/registration/quat/all.hh (working copy) @@ -2,7 +2,6 @@ # define QUAT_ALL_HH -# include "interpol.hh" # include "rotation.hh" Index: sandbox/jardonnet/registration/quat/rotation.hh --- sandbox/jardonnet/registration/quat/rotation.hh (revision 1791) +++ sandbox/jardonnet/registration/quat/rotation.hh (working copy) @@ -10,26 +10,30 @@ # include <mln/algebra/vec.hh> # include <mln/make/vec.hh> # include <mln/make/mat.hh> -# include <mln/value/quat.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 { - vec3f rotate(const value::quat& q, const vec3f& p) + // 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 * value::quat(0. ,p) * q.inv()).v(); + return (q * algebra::quat(0. ,p) * q.inv()).v(); } - - bool check_rotation(const algebra::mat<3,3,float>& mat, - const value::quat& q) + //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()); - vec3f + algebra::vec<n,float> tmp = make::vec(rand(), rand(), rand()), p = tmp / norm::l2(tmp), p_rot_1 = rotate(q, p), @@ -38,7 +42,8 @@ } - algebra::mat<3,3,float> quat2mat(const value::quat& q) + //FIXME : switch to n dim. + algebra::mat<3,3,float> quat2mat(const algebra::quat& q) { assert(q.is_unit()); float