[Olena-patches] 1848: Sandbox: ICP: various tools and improvments.

https://svn.lrde.epita.fr/svn/oln/trunk/milena Index: ChangeLog from Ugo Jardonnet &lt;ugo.jardonnet(a)lrde.epita.fr&gt; Sandbox: ICP: various tools and improvments. * sandbox/jardonnet/test/icp.cc: Fix regression (output registred.pbm). * sandbox/jardonnet/registration/tools.hh: Add multiple 2d to 3d and 3d to 2d conversion . * sandbox/jardonnet/registration/quat7.hh: Rewrite Covariance matrix computation. * sandbox/jardonnet/registration/frankel_young.hh: New, resolve linear equation Ax = b. Useless for icp. * sandbox/jardonnet/registration/power_it.hh: Find greatest eigen value, should be more efficient than jacobi ?. * sandbox/jardonnet/registration/icp.hh: Add intermediate image outputs. mln/algebra/quat.hh | 2 sandbox/jardonnet/registration/cloud.hh | 2 sandbox/jardonnet/registration/frankel_young.hh | 46 ++++++++++ sandbox/jardonnet/registration/icp.hh | 14 ++- sandbox/jardonnet/registration/jacobi.hh | 6 - sandbox/jardonnet/registration/power_it.hh | 41 +++++++++ sandbox/jardonnet/registration/quat7.hh | 38 +++++++- sandbox/jardonnet/registration/tools.hh | 109 ++++++++++++++++-------- sandbox/jardonnet/test/icp.cc | 33 +++---- 9 files changed, 233 insertions(+), 58 deletions(-) Index: mln/algebra/quat.hh --- mln/algebra/quat.hh (revision 1847) +++ mln/algebra/quat.hh (working copy) @@ -186,7 +186,7 @@ quat conj() const; /// Give the invert. - quat inv() const; // FIXME: Rename inv as invert. + quat inv() const; //FIXME: rename invert. /// Transform into unit quaternion. quat& set_unit(); Index: sandbox/jardonnet/test/icp.cc --- sandbox/jardonnet/test/icp.cc (revision 1847) +++ sandbox/jardonnet/test/icp.cc (working copy) @@ -41,27 +41,30 @@ closest_point<mln_point_(I3d)> fun(x, box_<point3d>(1000,1000,1)); lazy_image< closest_point<mln_point_(I3d)> > map(fun); - registration::icp(c, map); + quat7<3> q = registration::icp(c, map); #ifndef NDEBUG std::cout << "closest_point(Ck[i]) = " << fun.i << std::endl; std::cout << "Pts processed = " << registration::pts << std::endl; #endif - // //init output image - // //FIXME: Should be - // //mln_concrete(I) output(res.bbox()) = convert::to_image<I>(res)? - // const box_<point2d> box2d(1000,1000,0); - // image2d<bool> output(box2d, 1); - - // //to 2d : projection (FIXME:if 3d) - // for (size_t i = 0; i < res.npoints(); i++) - // { - // point2d p(res[i][0], res[i][1]); - // if (output.has(p)) - // output(p) = true; - // } + //init output image + //FIXME: Should be + //mln_concrete(I) output(res.bbox()) = convert::to_image<I>(res)? + + q.apply_on(c, c); + + const box_<point2d> box2d(1000,1000); + image2d<bool> output(box2d, 1); + + //to 2d : projection (FIXME:if 3d) + for (size_t i = 0; i < c.npoints(); i++) + { + point2d p(c[i][0], c[i][1]); + if (output.has(p)) + output(p) = true; + } - //io::pbm::save("./+registred.pbm"); + io::pbm::save(output, "registred.pbm"); } Index: sandbox/jardonnet/registration/jacobi.hh --- sandbox/jardonnet/registration/jacobi.hh (revision 1847) +++ sandbox/jardonnet/registration/jacobi.hh (working copy) @@ -17,7 +17,7 @@ #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(algebra::mat<4,4,float> a, algebra::quat& q) + algebra::quat jacobi(algebra::mat<4,4,float> a) { float dd, d[4]; algebra::mat<4,4,float> v(literal::zero); @@ -46,12 +46,12 @@ iq = ip; dd = d[ip]; } - q = algebra::quat(v(0,iq), + algebra::quat q(v(0,iq), v(1,iq), v(2,iq), v(3,iq)); q.set_unit(); - return; + return q; } if (i < 4) { i++; Index: sandbox/jardonnet/registration/power_it.hh --- sandbox/jardonnet/registration/power_it.hh (revision 0) +++ sandbox/jardonnet/registration/power_it.hh (revision 0) @@ -0,0 +1,41 @@ +#ifndef POWER_IT_HH +# define POWER_IT_HH + + +#include <mln/algebra/mat.hh> +#include <cmath> +#include "misc.hh" + +// from num. rec. in C + + +namespace mln +{ + + algebra::quat power_it(algebra::mat<4,4,float> A) + { + float normex = 1.; + + algebra::vec<4,float> x0(literal::zero); + algebra::vec<4,float> b(literal::zero); + + algebra::vec<4,float> x(literal::zero); + for (int i = 0; i < 4; i++) + x[i] = 1.; + + while (fabs(norm::l2(x) - norm::l2(x0)) > 1e-6) + { + x0 = x; + normex = norm::l2(x); + b = x / normex; + x = A * b; + } + normex = norm::l2(x); + + algebra::quat q(x / normex); + q.set_unit(); + return q; + } +} + +#endif /* POWER_IT_HH */ Index: sandbox/jardonnet/registration/tools.hh --- sandbox/jardonnet/registration/tools.hh (revision 1847) +++ sandbox/jardonnet/registration/tools.hh (working copy) @@ -17,12 +17,17 @@ typedef P result; closest_point(const p_array<P>& X, const box_<P>& box) - : X(X), box(box), i(0) + : X(X), box(box) + +#ifndef NDEBUG + , i(0) +#endif + { } result //inline - operator () (const P& Ck) + operator () (const P& Ck) const { #ifndef NDEBUG @@ -45,14 +50,17 @@ return algebra::to_point<P>(best_x); } - const box_<P>& domain() + const box_<P>& domain() const { return box; } const p_array<P>& X; const box_<P> box; + +#ifndef NDEBUG mutable unsigned i; +#endif }; @@ -74,7 +82,7 @@ const mln_result(F) //inline - operator() (const typename F::input& p) + operator () (const typename F::input& p) const { mln_precondition(fun.domain().has(p)); //FIXME: What about domain? @@ -86,31 +94,12 @@ } //FIXME: 3d -> //mln_dim(ml_input(input)) - image3d<mln_result(F)> value; - image3d<bool> is_known; + mutable image3d<mln_result(F)> value; + mutable image3d<bool> is_known; F& fun; }; -// // Point - -// template <typename P> -// P min(const P& a, const P& b) -// { -// if (a > b) -// return b; -// return a; -// } - -// template <typename P> -// P max(const P& a, const P& b) -// { -// if (a < b) -// return b; -// return a; -// } - - // Box template <typename P> @@ -142,6 +131,7 @@ namespace convert { + // to_p_array template <typename I> inline p_array<mln_point(I)> @@ -158,23 +148,74 @@ return a; } + template < typename P > + inline + box_<point2d> + to_box2d(const box_<P>& b) + { + point2d pmin(b.pmin()[0], b.pmin()[1]); + point2d pmax(b.pmax()[0], b.pmax()[1]); + + return box_<point2d>(pmin, pmax); + } + + //FIXME: to write - /* - template <typename A> + template <typename P> inline - image2d<mln_value(A)> - to_image2d(const A& a) + image2d<bool> + to_image2d(const p_array<P>& a) { - image2d<mln_value(A)> img(a.bbox()); + image2d<bool> output (to_box2d(a.bbox()), 0); for (size_t i = 0; i < a.npoints(); i++) { - point2d p(res[i][0], res[i][1]); - //FIXME: BOF - //if (output.has(p)) + point2d p(a[i][0], a[i][1]); + if (output.has(p)) output(p) = true; } + return output; + } + + // to_pointNd + + //FIXME: Should we really provide this + //point3d -> point2d + template <typename T> + inline + point_<grid::square, T> + to_point2d(const point_<grid::cube, T>& p) + { + return point_<grid::square, T>(p[0], p[1]); + } + //point2d -> point2d + template <typename T> + inline + point_<grid::square, T>& + to_point2d(const point_<grid::square, T>& p) + { + return p; + } + + //point2d -> point3d + template <typename T> + inline + point_<grid::cube, T> + to_point3d(const point_<grid::square, T>& p) + { + return point_<grid::cube, T>(p[0], p[1], 0); + } + //point3d -> point3d + template <typename T> + inline + point_<grid::cube, T>& + to_point3d(const point_<grid::cube, T>& p) + { + return p; } - */ + + + // to_imageNd + //const return a const template <typename T> Index: sandbox/jardonnet/registration/quat7.hh --- sandbox/jardonnet/registration/quat7.hh (revision 1847) +++ sandbox/jardonnet/registration/quat7.hh (working copy) @@ -10,6 +10,8 @@ # include "rotation.hh" # include "jacobi.hh" +# include "power_it.hh" +# include "frankel_young.hh" # include <mln/norm/l2.hh> @@ -115,6 +117,8 @@ } Mk /= C.npoints(); + + /* 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)}; @@ -127,14 +131,42 @@ 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(literal::zero); + algebra::vec<3,float> a; + a[0] = Mk(1,2) - Mk(2,1); + a[1] = Mk(2,0) - Mk(0,2); + a[2] = Mk(0,1) - Mk(1,0); + + algebra::mat<4,4,float> Qk(literal::zero); + float t = tr(Mk); + + Qk(0,0) = t; + for (int i = 1; i < 4; i++) + { + Qk(i,0) = a[i - 1]; + Qk(0,i) = a[i - 1]; + for (int j = 1; j < 4; j++) + if (i == j) + Qk(i,j) = 2 * Mk(i - 1,i - 1) - t; + } + Qk(1,2) = Mk(0,1) + Mk(1,0); + Qk(2,1) = Mk(0,1) + Mk(1,0); + + Qk(1,3) = Mk(0,2) + Mk(2,0); + Qk(3,1) = Mk(0,2) + Mk(2,0); + + Qk(2,3) = Mk(1,2) + Mk(2,1); + Qk(3,2) = Mk(1,2) + Mk(2,1); + + algebra::quat qR(literal::zero); + qR = jacobi(Qk); + //std::cout << qR << std::endl; + //qR = power_it(Qk); //std::cout << qR << std::endl; - jacobi(Qk, qR); // qT - const algebra::vec<P::dim,float> qT = mu_Xk - rotate(qR, mu_C); return quat7<P::dim>(qR, qT); Index: sandbox/jardonnet/registration/frankel_young.hh --- sandbox/jardonnet/registration/frankel_young.hh (revision 0) +++ sandbox/jardonnet/registration/frankel_young.hh (revision 0) @@ -0,0 +1,46 @@ +#ifndef _FRANKEL_YOUNG_HH +# define _FRANKEL_YOUNG_HH + +//Successive over-relaxation (SOR) is a numerical method used to speed up +//convergence of the Gauss-Seidel method for solving a linear system of +//equations. + +namespace mln +{ + + algebra::quat frankel_young(algebra::mat<4,4,float> a, float w) + { + //Inputs: A , b, w + //Output: phi + + algebra::vec<4,float> phi(literal::zero); + algebra::vec<4,float> old_phi(literal::zero); + + algebra::vec<4,float> b; + for (int i = 0; i < 4; i++) + b[i] = phi[i] = a(i,i); + + //Choose an initial guess phi to the solution + while (fabs(norm::l2(old_phi) - norm::l2(phi)) > 1e-6) + { + old_phi = phi; + for (int i = 1; i < 4; i++) + { + float sigma = 0; + + for (int j = 1; j < i-1; j++) + sigma += a(i,j) * phi[i]; + + for (int j = i + 1; j < 4; j++) + sigma += a(i,j) * phi[i]; + + phi[i] = (1 - w) * phi[i] + w / a(i,i) * (b[i] - sigma); + } + } + algebra::quat q(phi); + q.set_unit(); + return q; + } +} + +#endif /* _FRANKEL_YOUNG_HH */ Index: sandbox/jardonnet/registration/cloud.hh --- sandbox/jardonnet/registration/cloud.hh (revision 1847) +++ sandbox/jardonnet/registration/cloud.hh (working copy) @@ -56,7 +56,7 @@ { algebra::vec<3,float> a1f = a1[i]; algebra::vec<3,float> a2f = a2[i]; - f += norm::l2_distance(a1f,a2f); + f += norm::l2(a1f - a2f); } return f / a1.npoints(); } Index: sandbox/jardonnet/registration/icp.hh --- sandbox/jardonnet/registration/icp.hh (revision 1847) +++ sandbox/jardonnet/registration/icp.hh (working copy) @@ -33,6 +33,9 @@ * \brief image registration */ +# include <iostream> +# include <string> + # include <mln/algebra/quat.hh> # include <mln/algebra/vec.hh> # include <mln/make/w_window.hh> @@ -111,6 +114,15 @@ err = rms(Ck, Xk); #ifndef NDEBUG + + { + using namespace std; + image2d<bool> img = convert::to_image2d(Ck); + stringstream oss; + oss << "reg" << k << ".pbm"; + io::pbm::save(img, oss.str()); + } + //plot file std::cout << k << '\t' << err << '\t' << (qk - old_qk).sqr_norm() << '\t' @@ -138,8 +150,8 @@ { trace::entering("registration::icp"); - mln_precondition(cloud.npoints() != 0); mln_precondition(P::dim == 3); + mln_precondition(cloud.npoints() != 0); //init rigid transform qk quat7<P::dim> qk;