* mln/registration/icp2.hh: Make it work and make it more generic.
---
 milena/ChangeLog                |    6 +
 milena/mln/registration/icp2.hh |  334 +++++++++++++++++++++++++++++----------
 2 files changed, 256 insertions(+), 84 deletions(-)
diff --git a/milena/ChangeLog b/milena/ChangeLog
index da694aa..5669206 100644
--- a/milena/ChangeLog
+++ b/milena/ChangeLog
@@ -1,5 +1,11 @@
 2009-02-03  Guillaume Lazzara  <z(a)lrde.epita.fr>
 
+	Update new version of ICP.
+
+	* mln/registration/icp2.hh: Make it work and make it more generic.
+
+2009-02-03  Guillaume Lazzara  <z(a)lrde.epita.fr>
+
 	Fix distribution.
 
 	* headers.mk: add mln/io/cloud/all.hh.
diff --git a/milena/mln/registration/icp2.hh b/milena/mln/registration/icp2.hh
index e077b4e..2993caa 100644
--- a/milena/mln/registration/icp2.hh
+++ b/milena/mln/registration/icp2.hh
@@ -32,93 +32,224 @@
 ///
 /// Register an image over an another using the ICP algorithm.
 
+# include <mln/core/alias/vec3d.hh>
+# include <mln/math/jacobi.hh>
 # include <mln/fun/x2x/all.hh>
 # include <mln/fun/x2v/all.hh>
 # include <mln/convert/to.hh>
-# include <mln/transform/internal/closest_point_functor.hh>
-# include <mln/canvas/distance_geodesic.hh>
 # include <mln/accu/compute.hh>
 # include <mln/accu/center.hh>
+# include <mln/accu/rms.hh>
+# include <mln/trait/image_from_grid.hh>
+# include <mln/set/compute.hh>
+
+//Should be removed when closest_point functors are moved.
+# include <mln/core/alias/neighb3d.hh>
+# include <mln/transform/internal/closest_point_functor.hh>
+# include <mln/canvas/distance_geodesic.hh>
+# include <mln/pw/all.hh>
 
 namespace mln
 {
 
+
   namespace registration
   {
 
     using namespace fun::x2x;
 
-    /*! Register point in \p c using a map of closest points \p map
+    /*! Register point in \p c using a function of closest points
+     * \p closest_point.
      *
-     * \param[in] c The cloud of points.
-     * \param[in] map The map of closest points.
-     * \param[in] epsilon ICP stops if sqr_norm(qk - qk-1) /
-     * sqr_norm(qk) > epsilon
+     * \param[in] data_P The cloud of points.
+     * \param[in] model_X the reference surface.
+     * \param[in] closest_point The function of closest points.
      * \param[out] qk The rigid transformation obtained.
      *
-     * \pre \p ima has to be initialized.
+     * \return the rigid transformation which may be use later to create
+     * a registered image.
+     *
+     * WARNING: the function \p closest_point *MUST* take float/double
+     * vector as arguments. Otherwise the resulting transformation may be
+     * wrong due to the truncation of the vector coordinate values.
+     *
+     * \pre \p data_p and \p model_X must not be empty.
+     *
+     * Reference article: "A Method for Registration of 3-D Shapes", Paul J.
+     * Besl and Neil D. McKay, IEEE, 2, February 1992.
+     *
      */
-    template <typename D, typename M, typename T>
-    void
-    icp(const Image<D>& data, const Image<M>& model, T& qk);
+    template <typename P, typename F>
+    composed< rotation<P::dim,float>,translation<P::dim,float> >
+    icp(const p_array<P>& data_P,
+	const p_array<P>& model_X,
+	const F& closest_point,
+	const algebra::quat& initial_rot,
+	const mln_vec(P)& initial_translation);
+
+    template <typename P, typename F>
+    composed< rotation<P::dim,float>,translation<P::dim,float> >
+    icp(const p_array<P>& data_P,
+	const p_array<P>& model_X,
+	const F& closest_point);
 
 # ifndef MLN_INCLUDE_ONLY
 
-    template <typename P, typename C, typename T>
+    /// Closest point functor based on map distance.
+    template <typename P>
+    class cp_with_map_t
+    {
+      typedef mln_image_from_grid(mln_grid(P), P) I;
+      typedef mln_ch_value(I, P) cp_ima_t;
+
+    public:
+
+      cp_with_map_t(const p_array<P>& model_X)
+      {
+	box3d box = geom::bbox(model_X);
+	box.enlarge(50);
+
+	typedef mln_ch_value(I, bool) model_t;
+	model_t model(box);
+	data::fill(model, false);
+	data::fill((model | model_X).rw(), true);
+
+	transform::internal::closest_point_functor<I> f;
+	typedef mln_ch_value(I,value::int_u16) dmap_t;
+	dmap_t dmap_X = canvas::distance_geodesic(model, c6(),
+						  mln_max(value::int_u16),
+						  f);
+
+	cp_ima_ = f.cp_ima;
+      }
+
+      mln_site(I) operator()(const mln_site(I)& p) const
+      {
+	return cp_ima_(p);
+      }
+
+    private:
+      cp_ima_t cp_ima_;
+    };
+
+
+    /// Closest point functor based on map distance.
+    template <typename P>
+    class basic_closest_point
+    {
+      typedef mln_image_from_grid(mln_grid(P), P) I;
+      typedef p_array<P> X_t;
+
+    public:
+
+      basic_closest_point(const p_array<P>& X)
+	: X_(X)
+      {
+      }
+
+      float
+      l2_distance(const vec3d_f& vec1, const vec3d_f& vec2) const
+      {
+	typedef float D;
+	D d = 0;
+	for (unsigned i = 0; i < 3; ++i)
+	  {
+	    D sqr_v1_v2 = static_cast<D>(mln::math::sqr(vec1[i] - vec2[i]));
+	    d = static_cast<D>(d + sqr_v1_v2);
+	  }
+	return d;
+      }
+
+      mln_site(I) operator()(const vec3d_f& v) const
+      {
+	vec3d_f best_x = convert::to<vec3d_f>(X_[0].to_vec());
+
+	float best_d = l2_distance(v, best_x);
+	mln_piter(X_t) X_i(X_);
+	for_all(X_i)
+	{
+	  float d = l2_distance(v, convert::to<vec3d_f>(X_i));
+	  if (d < best_d)
+	  {
+	    best_d = d;
+	    best_x = X_i.to_vec();
+	  }
+	}
+	return best_x;
+      }
+
+    private:
+	const p_array<P>& X_;
+    };
+
+
+    template <typename P, typename F>
     inline
     float
     compute_d_k(const p_array<P>& data_P,
-		const C& cp_X,
-		const T& qk,
-		const T& qk_next)
+		const F& closest_point,
+		const algebra::quat& qR,
+		const algebra::quat& qR_old,
+		const vec3d_f& qT,
+		const vec3d_f& qT_old)
     {
-      float f = 0.f;
+      accu::rms<vec3d_f, float> accu;
       mln_piter(p_array<P>) p(data_P);
       for_all(p)
       {
 	// yk_i - pk+1_i
-        f += norm::l2(cp_X(qk(p.to_site().to_vec())).to_vec() - qk_next(p.to_site().to_vec()));
+	vec3d_f Pk_i = qR_old.rotate(p.to_vec()) + qT_old;
+	vec3d_f Pk_1_i = qR.rotate(p.to_vec()) + qT;
+        accu.take(closest_point(Pk_i).to_vec() - Pk_1_i);
       }
-      return f / data_P.nsites();
+      return accu.to_result();
     }
 
-    template <typename P, typename C, typename T>
+    template <typename P, typename F>
     inline
     float
     compute_e_k(const p_array<P>& data_P,
-		const C& cp_X,
-		const T& qk)
+		const F& closest_point,
+		const algebra::quat& qR,
+		const vec3d_f& qT)
     {
-      float f = 0.f;
+      accu::rms<vec3d_f, float> accu;
       mln_piter(p_array<P>) p(data_P);
       for_all(p)
       {
 	// yk_i - pk_i
-        f += norm::l2(cp_X(qk(p.to_site().to_vec())).to_vec() - qk(p.to_site().to_vec()));
+	vec3d_f Pk_i = qR.rotate(p.to_vec()) + qT;
+        accu.take(closest_point(Pk_i).to_vec() - Pk_i);
       }
-      return f / data_P.nsites();
+      return accu.to_result();
     }
 
 
-    template <typename P, typename T>
-    fun::x2x::rotation<3u, float>
+    /// FIXME: work only for 3d images.
+    template <typename P, typename F>
+    algebra::quat
     get_rot(const p_array<P>& data_P,
-            const algebra::vec<3u,float>& mu_P,
-            const algebra::vec<3u,float>& mu_Yk,
-	    const T& qk)
+            const vec3d_f& mu_P,
+            const vec3d_f& mu_Yk,
+	    const F& closest_point,
+	    const algebra::quat& qR,
+	    const vec3d_f& qT)
     {
-      // FIXME: Make use of a cross_covariance accu (maybe not because of map(ck[i]))
+      /// Mk: cross-covariance matrix.
       algebra::mat<3u,3u,float> Mk;
       mln_piter(p_array<P>) p(data_P);
+
+      // FIXME: could we use an accu?
       for_all(p)
       {
-	algebra::vec<3u,float> P_i  = p.to_site().to_vec();
-	algebra::vec<3u,float> Yk_i = qk(p.to_site().to_vec());
+	vec3d_f P_i  = p.to_vec();
+	vec3d_f Pk_i = qR.rotate(p.to_vec()) + qT;
+	vec3d_f Yk_i = closest_point(Pk_i).to_vec();
 	Mk += make::mat(P_i - mu_P) * trans(make::mat(Yk_i - mu_Yk));
       }
       Mk /= data_P.nsites();
 
-      algebra::vec<3u,float> A;
+      vec3d_f 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);
@@ -127,11 +258,11 @@ namespace mln
       float t = tr(Mk);
 
       Qk(0,0) = t;
-      for (int i = 1; i < 4; i++)
+      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++)
+        for (int j = 1; j < 4; ++j)
           if (i == j)
             Qk(i,j) = 2 * Mk(i - 1,i - 1) - t;
       }
@@ -145,80 +276,115 @@ namespace mln
       Qk(2,3) = Mk(1,2) + Mk(2,1);
       Qk(3,2) = Mk(1,2) + Mk(2,1);
 
-      algebra::quat qR = math::jacobi(Qk);
-
-      return fun::x2x::rotation<3u, float>(qR);
+      return mln::math::jacobi(Qk);
     }
 
 
-
-    template <typename P, typename C, typename T>
-    T
-    get_transformation(const p_array<P>& data_P,
-		       const algebra::vec<P::dim,float>& mu_P,
-		       const C& cp_X,
-		       const T& qk)
+    // Compute mu_Yk, mass center of Yk.
+    template <typename P, typename F>
+    inline
+    vec3d_f
+    get_mu_yk(const p_array<P>& data_P,
+	      const F& closest_point,
+	      const algebra::quat& qR,
+	      const vec3d_f& qT)
     {
-      algebra::vec<P::dim,float> mu_Yk;
+      accu::center<P,vec3d_f> accu;
 
-      // Compute mu_Yk, mass center of Yk.
       mln_piter(p_array<P>) p(data_P);
       for_all(p)
-        mu_Yk += cp_X(qk(p.to_site().to_vec())).to_vec();
-      mu_Yk /= data_P.nsites();
-
-      // qR
-      rotation<P::dim, float> tqR = get_rot(data_P, mu_P, mu_Yk, qk);
-
-      // qT
-      const algebra::vec<P::dim,float> qT = mu_Yk - tqR(mu_P);
-      translation<P::dim, float> tqT(qT);
-
-      return compose(tqR, tqT);
+      {
+	vec3d_f Pk_i = qR.rotate(p.to_vec()) + qT;
+	accu.take(closest_point(Pk_i).to_vec());
+      }
+      return accu.to_result();
     }
 
 
-    template <typename D, typename M, typename T>
+    template <typename P, typename F>
     inline
-    void
-    icp(const Image<D>& data, const Image<M>& model_X, T& qk)
+    composed< rotation<P::dim,float>,translation<P::dim,float> >
+    icp(const p_array<P>& data_P,
+	const p_array<P>& model_X,
+	const F& closest_point,
+	const algebra::quat& initial_rot,
+	const mln_vec(P)& initial_translation)
     {
       trace::entering("registration::icp");
 
-      typedef mln_psite(M) P;
+      mln_precondition(P::dim == 3);
+      mln_precondition(!data_P.is_empty());
+      mln_precondition(!model_X.is_empty());
+      mln_precondition(!initial_rot.is_null());
+
       typedef p_array<P> cloud_t;
 
-      cloud_t data_P = convert::to<cloud_t>(exact(data));
+      vec3d_f mu_P = set::compute(accu::center<P,vec3d_f>(), data_P);
+
+      vec3d_f qT_old, qT = initial_translation;
+      algebra::quat qR_old, qR = initial_rot;
+      float ek, ek_old = mln_max(float);
+      float dk, dk_old = mln_max(float);
+      unsigned k = 0;
+      do
+      {
+	qT_old = qT;
+	qR_old = qR;
 
-      // Compute distance map and closest point image.
-      transform::internal::closest_point_functor<M> f;
-      typedef mln_ch_value(M,value::int_u16) I;
-      I dmap_X = canvas::distance_geodesic(model_X, c6(),
-					   mln_max(value::int_u16), f);
+	// Compute error ek = d(Pk,Yk)
+	ek = compute_e_k(data_P, closest_point, qR, qT);
 
-      const mln_ch_value(M, P)& cp_X = f.cp_ima;
+      /// Compute transformation
+      ///
+	// mu_Yk - Pk's mass center.
+	vec3d_f mu_Yk = get_mu_yk(data_P, closest_point, qR_old, qT_old);
 
-      algebra::vec<P::dim,float> mu_P = set::compute(accu::meta::center(),
-						     exact(data).domain()).to_vec();
+	// quaternion qR - rotation
+	qR = get_rot(data_P, mu_P, mu_Yk, closest_point, qR_old, qT_old);
+	vec3d_f tmp = qR.v();
 
-      float d_k_prev = 0, d_k = mln_max(float);
-      float e_k_prev = 0, e_k = mln_max(float);
-      do
-      {
-	d_k_prev = d_k;
-	e_k_prev = e_k;
+	// vector qT - translation
+	qT = mu_Yk - qR.rotate(mu_P);
+      ///
+      /// End of "compute transformation"
 
-	//compute qk
-	T qk_next = get_transformation(data_P, mu_P, cp_X, qk);
+	// Distance dk = d(Pk+1, Yk)
+	dk = compute_d_k(data_P, closest_point, qR, qR_old, qT, qT_old);
 
-	d_k = compute_d_k(data_P, cp_X, qk, qk_next);
-	std::cout << d_k << std::endl;
-	e_k = compute_e_k(data_P, cp_X, qk);
+	// Check property according the related paper.
+	mln_assertion(0 <= dk);
+	mln_assertion(dk <= ek);
+	mln_assertion(ek <= dk_old);
+	mln_assertion(dk_old <= ek_old);
 
-	qk = qk_next;
-      } while ((d_k_prev - d_k) >= 1e-3);
+	// Backing up results.
+	dk_old = dk;
+	ek_old = ek;
+
+	++k;
+
+      } while ((k < 3)
+	  || norm::l2((qT - qT_old)) + norm::l2((qR - qR_old).to_vec()) > 1e-5);
+
+      typedef rotation<3u,float> rot_t;
+      rot_t tqR(qR);
+      typedef translation<3u,float> trans_t;
+      trans_t tqT(qT);
+      composed<rot_t,trans_t> result(tqR, tqT);
 
       trace::exiting("registration::icp");
+      return result;
+    }
+
+    template <typename P, typename F>
+    inline
+    composed< rotation<P::dim,float>,translation<P::dim,float> >
+    icp(const p_array<P>& data_P,
+	const p_array<P>& model_X,
+	const F& closest_point)
+    {
+      return icp(data_P, model_X, closest_point,
+		 algebra::quat(1,0,0,0), literal::zero);
     }
 
 # endif // ! MLN_INCLUDE_ONLY
-- 
1.5.6.5