[Olena-patches] 1867: Sandbox: ICP: Qk driving.

https://svn.lrde.epita.fr/svn/oln/trunk/milena Index: ChangeLog from Ugo Jardonnet &lt;ugo.jardonnet(a)lrde.epita.fr&gt; Sandbox: ICP: Qk driving. * sandbox/jardonnet/test/Makefile: Add rules. * sandbox/jardonnet/TODO: Update. * sandbox/jardonnet/registration/interpolation.hh: give coef of the interpolated polynome. * sandbox/jardonnet/registration/quat7.hh: Update interface */-+. * sandbox/jardonnet/registration/cloud.hh: Update center. * sandbox/jardonnet/registration/icp.hh: Add Optimization on qk. * sandbox/jardonnet/registration/update_qk.hh: Optimization on qk. TODO | 8 ++++ registration/cloud.hh | 12 +------ registration/icp.hh | 28 ++++++++++++----- registration/interpolation.hh | 41 +++++++++++++++++++++++++ registration/quat7.hh | 27 ++++++++++++++-- registration/update_qk.hh | 68 ++++++++++++++++++++++++++++++++++++++++++ test/Makefile | 19 ++++++++--- test/plotscript | 2 - 8 files changed, 180 insertions(+), 25 deletions(-) Index: sandbox/jardonnet/test/plotscript --- sandbox/jardonnet/test/plotscript (revision 1866) +++ sandbox/jardonnet/test/plotscript (working copy) @@ -2,4 +2,4 @@ set ylabel "e" set zlabel "time" splot "log.dat" -pause 10 +pause -1 Index: sandbox/jardonnet/test/Makefile --- sandbox/jardonnet/test/Makefile (revision 1866) +++ sandbox/jardonnet/test/Makefile (working copy) @@ -13,17 +13,27 @@ gau: g++ gaussian.cc $(FLAG) -o 'gau' -icpD: icp.cc +icpD: icp.cc +depend g++ icp.cc -I../../.. -g -o 'icpD' -icp: icp.cc - g++ icp.cc -I../../.. -O3 -DNDEBUG -o 'icp' + +icp: icp.cc +depend icp.o + g++ icp.o -I../../.. -O3 -DNDEBUG -o 'icp' + +icp.o: + g++ -c icp.cc -I../../.. -O3 -DNDEBUG bench: - ./check.rb + ruby bench.rb clean: rm -f -- ./bin/* rm -f sub gsub gau icpD icp rm -f log.dat registred.pbm + rm -f i_* + +.PHONY: check bench icpD + ++depend: + g++ -M icp.cc $(FLAG) > +depend -.PHONY: check \ No newline at end of file +include +depend \ No newline at end of file Index: sandbox/jardonnet/TODO --- sandbox/jardonnet/TODO (revision 1866) +++ sandbox/jardonnet/TODO (working copy) @@ -11,3 +11,11 @@ adapt test for threshold (old thresholding) - - + +La translation est mauvaise car certain point de Xk sont compt� plusieurs fois. /bof + +- - +write +trans : vec --> quat + v |-> m = trans(t) + Index: sandbox/jardonnet/registration/interpolation.hh --- sandbox/jardonnet/registration/interpolation.hh (revision 0) +++ sandbox/jardonnet/registration/interpolation.hh (revision 0) @@ -0,0 +1,41 @@ +#ifndef MLN_INTERPOLATION_HH +# define MLN_INTERPOLATION_HH + + +namespace mln +{ + + void polcoe(float x[], float y[], int n, + float cof[]) + { + int k,j,i; + float phi,ff,b; + std::vector<float> s(n,0); + + for (i = 0; i <= n; i++) + s[i] = cof[i] = 0.0; + s[n] = -x[0]; + for (i = 1; i <= n; i++) + { + for (j = n - i; j <= n - 1; j++) + s[j] -= x[i] * s[j+1]; + s[n] -= x[i]; + } + for (j = 0; j <= n; j++) + { + phi = n + 1; + for (k = n; k >= 1; k--) + phi = k * s[k] + x[j] * phi; + ff = y[j] / phi; + b = 1.0; + for (k = n; k >= 0; k--) + { + cof[k] += b * ff; + b = s[k] + x[j] * b; + } + } + } + +} + +#endif // ! MLN_INTERPOLATION_HH Index: sandbox/jardonnet/registration/quat7.hh --- sandbox/jardonnet/registration/quat7.hh (revision 1866) +++ sandbox/jardonnet/registration/quat7.hh (working copy) @@ -13,7 +13,7 @@ # include "power_it.hh" # include "frankel_young.hh" -# include <mln/norm/l2.hh> +# include <mln/norm/all.hh> # include <mln/trait/all.hh> @@ -46,11 +46,12 @@ return norm::l2(_qR.to_vec()) + norm::l2(_qT); } - quat7 operator-(const quat7& rhs) const + float norm() const { - return quat7(_qR - rhs._qR, _qT - rhs._qT); + return norm::l1(_qR.to_vec()) + norm::l1(_qT); } + // quat7 is an object-function algebra::vec<n,float> operator()(const algebra::vec<n,float>& v) const @@ -77,6 +78,26 @@ std::cout << q._qT << std::endl; return os; } + + quat7 operator*(float f) + { + return quat7(_qR * f, _qT * f); + } + + quat7 operator/(float f) + { + return quat7(_qR / f, _qT / f); + } + + quat7 operator-(const quat7& rhs) const + { + return quat7(_qR - rhs._qR, _qT - rhs._qT); + } + + quat7 operator+(const quat7& rhs) const + { + return quat7(_qR + rhs._qR, _qT + rhs._qT); + } }; Index: sandbox/jardonnet/registration/cloud.hh --- sandbox/jardonnet/registration/cloud.hh (revision 1866) +++ sandbox/jardonnet/registration/cloud.hh (working copy) @@ -41,24 +41,18 @@ return tmp; } - template <typename P> + template <typename P, typename M> float rms(const p_array<P>& a1, - const p_array<P>& a2, + M& map, const size_t length) { assert(length <= a1.npoints()); - assert(length <= 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 < length; ++i) { algebra::vec<3,float> a1f = a1[i]; - algebra::vec<3,float> a2f = a2[i]; + algebra::vec<3,float> a2f = map(a1[i]); f += norm::l2(a1f - a2f); } return f / a1.npoints(); Index: sandbox/jardonnet/registration/icp.hh --- sandbox/jardonnet/registration/icp.hh (revision 1866) +++ sandbox/jardonnet/registration/icp.hh (working copy) @@ -46,7 +46,7 @@ # include "cloud.hh" # include "quat7.hh" -# include "projection.hh" +# include "update_qk.hh" # include "chamfer.hh" namespace mln @@ -91,10 +91,13 @@ << c_length << " points" << std::endl; std::cout << "k\terror\tdqk" << std::endl; #endif - quat7<P::dim> old_qk; + + quat7<P::dim> buf_qk[3]; + float buf_dk[3]; + float err; //float err_bis; - p_array<P> Ck(C), Xk(C); //FIXME: Xk copy C + p_array<P> Ck(C); //FIXME: Xk copy C algebra::vec<P::dim,float> mu_C = center(C, c_length), mu_Xk; @@ -103,15 +106,27 @@ unsigned int k = 0; do { + //update buff dk + buf_dk[2] = buf_dk[1]; + buf_dk[1] = buf_dk[0]; + buf_dk[0] = err; + //compute qk - old_qk = qk; qk = match(C, mu_C, Ck, map, c_length); + //update buff qk + buf_qk[2] = buf_qk[1]; + buf_qk[1] = buf_qk[0]; + buf_qk[0] = qk; + + //update qk + qk = update_qk(buf_qk, buf_dk); + //Ck+1 = qk(C) qk.apply_on(C, Ck, c_length); //err = d(Ck+1,Xk) - err = rms(Ck, Xk, c_length); + err = rms(Ck, map, c_length); #ifndef NDEBUG { @@ -141,9 +156,8 @@ //count the number of points processed pts += c_length; #endif - k++; - } while (k < 3 || (qk - old_qk).sqr_norm() > epsilon); + } while (k < 3 || (qk - buf_qk[0]).sqr_norm() > epsilon); trace::exiting("registration::impl::icp_"); return Ck; Index: sandbox/jardonnet/registration/update_qk.hh --- sandbox/jardonnet/registration/update_qk.hh (revision 0) +++ sandbox/jardonnet/registration/update_qk.hh (revision 0) @@ -0,0 +1,68 @@ +#ifndef MLN_UPDATE_QK_HH +# define MLN_UPDATE_QK_HH + +#include "interpolation.hh" +#include "quat7.hh" + +namespace mln +{ + + float arc(quat7<3> dqk, quat7<3> dqk_1) + { + //algebra::mat + + return 0; + } + + + quat7<3> update_qk(quat7<3> qk[3], + float dk[3]) + { + static float tetak = 11; + static quat7<3> dqk; + + quat7<3> dqk_1 = dqk; + dqk = qk[0] - qk[1]; + + float tetak_1 = tetak; + tetak = arc(dqk, dqk_1); + + if (tetak < 10 and tetak_1 < 10) //10degrees + { + float vk[3]; + float dk[3]; + float coef[3]; + + vk[0] = 0; + vk[1] = - dqk.norm(); + vk[2] = - dqk_1.norm() + vk[1]; + + float a1 = (dk[0] + dk[2]) / (vk[2] - vk[0]); + float b1 = dk[1] - ((dk[0] + dk[2]) / (vk[2] - vk[0])) * vk[1]; + float v1 = - b1 / a1; // > 0 + + polcoe(vk, dk, 3, coef); + float a2 = coef[0]; + float b2 = coef[1]; + float v2 = -b2 / (2. * a2); + + float vmax = 25 * dqk.norm(); + + if (0 < v2 and v2 < v1 and v1 < vmax) + return qk[0] + (dqk / dqk.norm()) * v2; + + if (0 < v1 and v1 < v2 and v2 < vmax) + return qk[0] + (dqk / dqk.norm()) * v1; + + if (v1 > vmax and v2 > vmax) + return qk[0] + (dqk / dqk.norm()) * vmax; + + + return qk[0]; + } + } + +} + + +#endif // ! MLN_UPDATE_QK_HH