[Olena-patches] 1833: Sandbox: ICP: Fix quaternion computation.

https://svn.lrde.epita.fr/svn/oln/trunk/milena Index: ChangeLog from Ugo Jardonnet &lt;ugo.jardonnet(a)lrde.epita.fr&gt; Sandbox: ICP: Fix quaternion computation. * sandbox/jardonnet/test/Makefile: Add rules for ICP. * sandbox/jardonnet/registration/quat7.hh (match): Fix matrix instantiation. * sandbox/jardonnet/registration/cloud.hh (rms): Make it use norm::l2. * sandbox/jardonnet/registration/jacobi.hh: Add safer checks, on float values. registration/cloud.hh | 14 ++++++++++++-- registration/icp.hh | 4 ---- registration/jacobi.hh | 13 +++++++++---- registration/quat7.hh | 8 +++++--- test/Makefile | 5 ++++- 5 files changed, 30 insertions(+), 14 deletions(-) Index: sandbox/jardonnet/test/Makefile --- sandbox/jardonnet/test/Makefile (revision 1832) +++ sandbox/jardonnet/test/Makefile (working copy) @@ -14,7 +14,10 @@ g++ gaussian.cc $(FLAG) -o '+gau.exe' icp: - g++ icp.cc -ffloat-store -I../../.. -O3 -DNDEBUG -o '+icp.exe' + g++ icp.cc -I../../.. -O1 -DNDEBUG -g -o '+icp.exe' + +icp++: + g++ icp.cc -I../../.. -O3 -DNDEBUG -o '+icp.exe' icpsafe: g++ icp.cc -fsignaling-nans -ffloat-store -I../../.. -O1 -o '+icp.exe' Index: sandbox/jardonnet/registration/quat7.hh --- sandbox/jardonnet/registration/quat7.hh (revision 1832) +++ sandbox/jardonnet/registration/quat7.hh (working copy) @@ -98,7 +98,7 @@ // qR - algebra::mat<P::dim,P::dim,float> Mk; + 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]; @@ -113,14 +113,16 @@ const algebra::mat<3,1,float> D = make::mat<3,1,3,float>(v); - algebra::mat<4,4,float> Qk; + algebra::mat<4,4,float> Qk(literal::zero); Qk(0,0) = tr(Mk); put(trans(D), 0,1, Qk); put(D, 1,0, Qk); put(Mk + trans(Mk) - algebra::mat<P::dim,P::dim,float>::identity() * tr(Mk), 1,1, Qk); - algebra::quat qR; + algebra::quat qR(literal::zero); + + //std::cout << qR << std::endl; jacobi(Qk, qR); // qT Index: sandbox/jardonnet/registration/cloud.hh --- sandbox/jardonnet/registration/cloud.hh (revision 1832) +++ sandbox/jardonnet/registration/cloud.hh (working copy) @@ -9,6 +9,7 @@ # include <mln/algebra/vec.hh> # include <mln/core/p_array.hh> +# include <mln/norm/l2.hh> namespace mln { @@ -32,9 +33,9 @@ // FIXME : move //exist for P? template <typename P> - mln_coord(P) sqr_norm(const P& v) + float sqr_norm(const P& v) { - mln_coord(P) tmp = 0; + float tmp = 0; for (unsigned i = 0; i < P::dim; i++) tmp += v[i] * v[i]; return tmp; @@ -45,9 +46,18 @@ const p_array<P>& a2) { assert(a1.npoints() == a2.npoints()); + /* float f = 0.f; for (size_t i = 0; i < a1.npoints(); ++i) f += sqr_norm(a1[i] - a2[i]); + return f / a1.npoints();*/ + float f = 0.f; + for (size_t i = 0; i < a1.npoints(); ++i) + { + algebra::vec<3,float> a1f = a1[i]; + algebra::vec<3,float> a2f = a2[i]; + f += norm::l2_distance(a1f,a2f); + } return f / a1.npoints(); } Index: sandbox/jardonnet/registration/jacobi.hh --- sandbox/jardonnet/registration/jacobi.hh (revision 1832) +++ sandbox/jardonnet/registration/jacobi.hh (working copy) @@ -4,6 +4,8 @@ #include <mln/algebra/mat.hh> +#include <cmath> +#include "misc.hh" // from num. rec. in C @@ -35,7 +37,7 @@ for (iq=ip+1;iq<4;iq++) sm += fabs(a(ip,iq)); } - if (sm < 1e-12) { + if (sm < 1e-6) { //1e-12 dd = d[0]; iq = 0; for (ip=1;ip<4;ip++) @@ -61,14 +63,17 @@ /* unusefull */ g=100.0*fabs(a(ip,iq)); - if (i > 4 && (float)(fabs(d[ip])+g) == (float)fabs(d[ip]) - && (float)(fabs(d[iq])+g) == (float)fabs(d[iq])) + //if (i > 4 && (float)(fabs(d[ip])+g) == (float)fabs(d[ip]) + // && (float)(fabs(d[iq])+g) == (float)fabs(d[iq])) + if (i > 4 && about_equal((float)(fabs(d[ip])+g), (float)fabs(d[ip])) + && about_equal((float)(fabs(d[iq])+g), (float)fabs(d[iq]))) a(ip,iq)=0.0; /* unusefull */ else if (fabs(a(ip,iq)) > tresh) { h=d[iq]-d[ip]; - if ((float)(fabs(h)+g) == (float)fabs(h)) + //if ((float)(fabs(h)+g) == (float)fabs(h)) // unsafe ? + if (about_equal((float)(fabs(h)+g), (float)fabs(h))) t=(a(ip,iq))/h; else { theta=0.5*h/(a(ip,iq)); Index: sandbox/jardonnet/registration/icp.hh --- sandbox/jardonnet/registration/icp.hh (revision 1832) +++ sandbox/jardonnet/registration/icp.hh (working copy) @@ -89,7 +89,6 @@ k = 0; do { - //std::cout << "step 2" << std::endl; //// step 2 projection::fill_Xk(Ck, map, Xk); @@ -98,16 +97,13 @@ 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