[Olena-patches] 1868: Sandbox: ICP: Fix 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: Fix Qk driving. * sandbox/jardonnet/test/bench: Add Qk driving log. * sandbox/jardonnet/registration/quat7.hh (to_vec): Add function. * sandbox/jardonnet/registration/icp.hh: Add qk/dk buffers. * sandbox/jardonnet/registration/update_qk.hh: Fix. registration/icp.hh | 16 +++++++----- registration/interpolation.hh | 2 - registration/quat7.hh | 54 +++++++++++++++++------------------------- registration/update_qk.hh | 54 ++++++++++++++++++++++-------------------- test/bench | 54 ++++++++++++++++++++++++++++++++++++++++++ test/icp.cc | 3 +- 6 files changed, 118 insertions(+), 65 deletions(-) Index: sandbox/jardonnet/test/bench --- sandbox/jardonnet/test/bench (revision 1867) +++ sandbox/jardonnet/test/bench (working copy) @@ -32,3 +32,57 @@ gprof icp_map 1 call chamfer 0.33s icp_lazy 16 call memo 0.21 + +**icp 01.pbm 02.pbm 1 1** +Register 943 points +k error dqk +0 18.9244 41.2757 +1 16.6149 21.4739 +2 15.2707 12.8804 +3 14.1741 10.3871 +4 13.4248 7.97125 +5 12.8224 7.72872 +6 12.5292 6.52063 +7 12.4356 4.50726 +8 12.3936 3.98019 +9 12.3728 3.42463 +10 12.3798 2.88974 +11 12.3838 2.42067 +12 12.3916 1.56065 +13 12.3627 1.42373 +14 12.3857 1.32764 +15 12.3562 1.2033 +16 12.3713 1.01804 +17 12.3657 0.672616 +18 12.3829 0.599551 +19 12.3859 0.423572 +20 12.3897 0.284606 +21 12.3894 0.183171 +22 12.3888 0.0807438 +closest_point(Ck[i]) = 12212 +Pts processed = 21689 +./icpD 01.pbm 02.pbm 1 1 18,19s user 0,05s system 97% cpu 18,619 total + + +**icp_drived 01.pbm 02.pbm 1 1** +Register 943 points +k error dqk +0 18.9244 41.2757 +1 16.6149 21.4739 +2 15.2707 12.8804 +3 14.1741 10.3871 +4 12.8711 15.9425 +5 12.3806 13.1241 +6 12.3601 3.38092 +7 12.3878 3.1961 +8 12.3975 5.48096 +9 12.3887 3.08797 +10 12.3611 2.27939 +11 12.3779 0.969556 +12 12.3955 0.581234 +13 12.3795 0.435324 +14 12.3931 0.0650237 +closest_point(Ck[i]) = 9847 +Pts processed = 14145 +./icpD 01.pbm 02.pbm 1 1 13,50s user 0,03s system 98% cpu 13,750 total + Index: sandbox/jardonnet/test/icp.cc --- sandbox/jardonnet/test/icp.cc (revision 1867) +++ sandbox/jardonnet/test/icp.cc (working copy) @@ -63,7 +63,7 @@ qk.apply_on(c, c, c.npoints()); - const box_<point2d> box2d(600,600); + const box_<point2d> box2d(400,700); image2d<bool> output(box2d, 1); //to 2d : projection (FIXME:if 3d) @@ -75,5 +75,6 @@ } io::pbm::save(output, "registred.pbm"); + } Index: sandbox/jardonnet/registration/interpolation.hh --- sandbox/jardonnet/registration/interpolation.hh (revision 1867) +++ sandbox/jardonnet/registration/interpolation.hh (working copy) @@ -10,7 +10,7 @@ { int k,j,i; float phi,ff,b; - std::vector<float> s(n,0); + std::vector<float> s(n); for (i = 0; i <= n; i++) s[i] = cof[i] = 0.0; Index: sandbox/jardonnet/registration/quat7.hh --- sandbox/jardonnet/registration/quat7.hh (revision 1867) +++ sandbox/jardonnet/registration/quat7.hh (working copy) @@ -79,6 +79,28 @@ return os; } + float operator[](int i) + { + if (i < n) + return _qT[i]; + else + return _qR.to_vec()[i - n]; + } + + algebra::vec<7,float> to_vec() + { + algebra::vec<7,float> v; + + for (int i = 0; i < 7; i++) + { + if (i < n) + v[i] = _qT[i]; + else + v[i] = _qR.to_vec()[i - n]; + } + return v; + } + quat7 operator*(float f) { return quat7(_qR * f, _qT * f); @@ -100,21 +122,6 @@ } }; - - // very usefull routine - - template <unsigned p, unsigned q, unsigned n, unsigned m> - void put(const algebra::mat<p,q,float>& in, // a matrix to put into... - unsigned row, unsigned col, // top-left location - algebra::mat<n,m,float>& inout) // ...a matrix to modify - { - assert(row + p <= n && col + q <= m); - for (unsigned i = 0; i < p; ++i) - for (unsigned j = 0; j < q; ++j) - inout(row + i, col + j) = in(i,j); - } - - template <typename P, typename M> quat7<P::dim> match(const p_array<P>& C, const algebra::vec<P::dim,float>& mu_C, @@ -124,7 +131,6 @@ { //FIXME: mix mu_Xk and qk loop - //mu_Xk = center map(Ck) algebra::vec<P::dim,float> mu_Xk(literal::zero); for (size_t i = 0; i < c_length; ++i) @@ -144,22 +150,6 @@ } Mk /= c_length; - - /* - 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); - - - 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::vec<3,float> a; a[0] = Mk(1,2) - Mk(2,1); a[1] = Mk(2,0) - Mk(0,2); Index: sandbox/jardonnet/registration/icp.hh --- sandbox/jardonnet/registration/icp.hh (revision 1867) +++ sandbox/jardonnet/registration/icp.hh (working copy) @@ -92,10 +92,10 @@ std::cout << "k\terror\tdqk" << std::endl; #endif - quat7<P::dim> buf_qk[3]; + quat7<P::dim> buf_qk[4]; float buf_dk[3]; - float err; + float err = 100; //float err_bis; p_array<P> Ck(C); //FIXME: Xk copy C @@ -106,7 +106,7 @@ unsigned int k = 0; do { - //update buff dk + //update buff dk //FIXME: rewrite it buf_dk[2] = buf_dk[1]; buf_dk[1] = buf_dk[0]; buf_dk[0] = err; @@ -114,13 +114,17 @@ //compute qk qk = match(C, mu_C, Ck, map, c_length); - //update buff qk + //update buff qk //FIXME: rewrite it + buf_qk[3] = buf_qk[2]; buf_qk[2] = buf_qk[1]; buf_qk[1] = buf_qk[0]; buf_qk[0] = qk; //update qk + if (k > 3) qk = update_qk(buf_qk, buf_dk); + qk._qR.set_unit(); + buf_qk[0] = qk; //Ck+1 = qk(C) qk.apply_on(C, Ck, c_length); @@ -151,13 +155,13 @@ //plot file std::cout << k << '\t' << err << '\t' - << (qk - old_qk).sqr_norm() << '\t' + << (qk - buf_qk[1]).sqr_norm() << '\t' << std::endl; //count the number of points processed pts += c_length; #endif k++; - } while (k < 3 || (qk - buf_qk[0]).sqr_norm() > epsilon); + } while (k < 3 || (qk - buf_qk[1]).sqr_norm() > epsilon); trace::exiting("registration::impl::icp_"); return Ck; Index: sandbox/jardonnet/registration/update_qk.hh --- sandbox/jardonnet/registration/update_qk.hh (revision 1867) +++ sandbox/jardonnet/registration/update_qk.hh (working copy) @@ -9,23 +9,23 @@ float arc(quat7<3> dqk, quat7<3> dqk_1) { - //algebra::mat - - return 0; + float res = 0.0; + for (int i = 0; i < 7; i++) + res += dqk[i] * dqk_1[i]; + res = res / (dqk.sqr_norm() * dqk_1.sqr_norm()); + return acos(res) * 180.0 / 3.14159265; } - quat7<3> update_qk(quat7<3> qk[3], + quat7<3> update_qk(quat7<3> qk[4], float dk[3]) { - static float tetak = 11; - static quat7<3> dqk; - - quat7<3> dqk_1 = dqk; - dqk = qk[0] - qk[1]; + quat7<3> dqk_2 = qk[2] - qk[3]; + quat7<3> dqk_1 = qk[1] - qk[2]; + quat7<3> dqk = qk[0] - qk[1]; - float tetak_1 = tetak; - tetak = arc(dqk, dqk_1); + float tetak_1 = arc(dqk_1, dqk_2); + float tetak = arc(dqk, dqk_1); if (tetak < 10 and tetak_1 < 10) //10degrees { @@ -34,33 +34,37 @@ float coef[3]; vk[0] = 0; - vk[1] = - dqk.norm(); - vk[2] = - dqk_1.norm() + vk[1]; + vk[1] = - dqk.sqr_norm(); + vk[2] = - dqk_1.sqr_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 + float a1 = (dk[2] + dk[0]) / (vk[2] - vk[0]); + float b1 = dk[1] - ((dk[2] + dk[0]) / (vk[2] - vk[0])) * vk[1]; + float v1 = math::abs(- b1 / a1); // > 0 - polcoe(vk, dk, 3, coef); + polcoe(vk, dk, 2, 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; + float vmax = 25 * dqk.sqr_norm(); - if (0 < v1 and v1 < v2 and v2 < vmax) - return qk[0] + (dqk / dqk.norm()) * v1; + if ((0 < v2 and v2 < v1 and v1 < vmax) or + (0 < v2 and v2 < vmax and vmax < v1)) + return qk[0] + (dqk / dqk.sqr_norm()) * v2; + + if ((0 < v1 and v1 < v2 and v2 < vmax) or + (0 < v1 and v1 < vmax and vmax < v2) or + (v2 < 0 and 0 < v1 and v1 < vmax)) + return qk[0] + (dqk / dqk.sqr_norm()) * v1; if (v1 > vmax and v2 > vmax) - return qk[0] + (dqk / dqk.norm()) * vmax; + return qk[0] + (dqk / dqk.sqr_norm()) * vmax; + std::cout << "echec" << std::endl; + } return qk[0]; } - } }