Milena (Olena)  User documentation 2.0a Id
 All Classes Namespaces Functions Variables Typedefs Enumerator Groups Pages
icp.hh
1 // Copyright (C) 2008, 2009, 2010, 2011 EPITA Research and Development
2 // Laboratory (LRDE)
3 //
4 // This file is part of Olena.
5 //
6 // Olena is free software: you can redistribute it and/or modify it under
7 // the terms of the GNU General Public License as published by the Free
8 // Software Foundation, version 2 of the License.
9 //
10 // Olena is distributed in the hope that it will be useful,
11 // but WITHOUT ANY WARRANTY; without even the implied warranty of
12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 // General Public License for more details.
14 //
15 // You should have received a copy of the GNU General Public License
16 // along with Olena. If not, see <http://www.gnu.org/licenses/>.
17 //
18 // As a special exception, you may use this file as part of a free
19 // software project without restriction. Specifically, if other files
20 // instantiate templates or use macros or inline functions from this
21 // file, or you compile this file and link it with other files to produce
22 // an executable, this file does not by itself cause the resulting
23 // executable to be covered by the GNU General Public License. This
24 // exception does not however invalidate any other reasons why the
25 // executable file might be covered by the GNU General Public License.
26 
27 #ifndef MLN_REGISTRATION_ICP_HH
28 # define MLN_REGISTRATION_ICP_HH
29 
35 
36 # include <cmath>
37 # include <algorithm>
38 
39 # include <mln/core/alias/vec3d.hh>
40 # include <mln/math/jacobi.hh>
41 # include <mln/fun/x2x/all.hh>
42 # include <mln/fun/x2v/all.hh>
43 # include <mln/convert/to.hh>
44 # include <mln/accu/compute.hh>
45 # include <mln/accu/center.hh>
46 # include <mln/accu/rms.hh>
47 # include <mln/trait/image_from_grid.hh>
48 # include <mln/set/compute.hh>
49 
50 //Should be removed when closest_point functors are moved.
51 # include <mln/core/image/dmorph/slice_image.hh>
52 # include <mln/core/image/imorph/tr_image.hh>
53 # include <mln/core/image/dmorph/extension_fun.hh>
54 
55 # include <mln/core/alias/neighb3d.hh>
56 
57 # include <mln/transform/distance_and_closest_point_geodesic.hh>
58 # include <mln/canvas/distance_geodesic.hh>
59 # include <mln/pw/all.hh>
60 
61 # include <mln/io/ppm/save.hh>
62 # include <mln/io/pbm/save.hh>
63 
64 # include <mln/labeling/colorize.hh>
65 # include <mln/debug/histo.hh>
66 
67 # include <mln/accu/histo.hh>
68 # include <mln/accu/math/sum.hh>
69 
70 # include <mln/value/int_u16.hh>
71 
72 # include <mln/literal/black.hh>
73 # include <mln/literal/white.hh>
74 # include <mln/literal/colors.hh>
75 
76 # include <mln/util/timer.hh>
77 
78 # include <mln/io/cloud/save.hh>
79 
80 
81 namespace mln
82 {
83 
84  namespace registration
85  {
86 
87  //FIXME: used for debug purpose. Should be removed later.
88 
89  using namespace fun::x2x;
90 
114  template <typename P, typename F>
115  std::pair<algebra::quat,mln_vec(P)>
116  icp(const p_array<P>& P_,
117  const p_array<P>& X,
118  const F& closest_point,
119  const algebra::quat& initial_rot,
120  const mln_vec(P)& initial_translation);
121 
122 
133  template <typename P, typename F>
134  composed< translation<P::dim,float>,rotation<P::dim,float> >
135  icp(const p_array<P>& P_,
136  const p_array<P>& X,
137  const F& closest_point);
138 
139 
140 # ifndef MLN_INCLUDE_ONLY
141 
142 
144  template <typename P>
146  {
147  typedef mln_image_from_grid(mln_grid(P), P) I;
148  typedef mln_ch_value(I, unsigned) cp_ima_t;
149  typedef mln_ch_value(I,value::int_u16) dmap_t;
150 
151  public:
152 
154  {
155  box3d box = geom::bbox(X);
156  box.enlarge(0, box.nslis());
157  box.enlarge(1, box.nrows());
158  box.enlarge(2, box.ncols());
159 
160  mln_postcondition(box.is_valid());
161 
162  std::cout << "Map image defined on " << box << std::endl;
163 
164  X_ = X;
165  init(X, box);
166  }
167 
168  closest_point_with_map(const p_array<P>& X, const box<P>& box)
169  {
170  X_ = X;
171  init(X, box);
172  }
173 
174  void init(const p_array<P>& X, const box<P>& box)
175  {
176  typedef mln_ch_value(I, bool) model_t;
177  model_t model(box);
178  data::fill(model, false);
179  data::fill((model | X).rw(), true);
180 
181 
182  typedef util::couple<mln_ch_value(model_t, value::int_u16),
183  mln_ch_value(model_t, unsigned)> couple_t;
185  c6(),
186  mln_max(value::int_u16));
187 
188  dmap_X_ = cpl.first();
189  cp_ima_ = cpl.second();
190 
191  mln_postcondition(cp_ima_.is_valid());
192  mln_postcondition(cp_ima_.domain().is_valid());
193  std::cout << "pmin = " << cp_ima_.domain().pmin() << std::endl;;
194  std::cout << "pmax = " << cp_ima_.domain().pmax() << std::endl;;
195 
196 #ifndef NDEBUG
197  mln_ch_value(I, bool) debug2(box);
198  data::fill(debug2, false);
199  mln_ch_value(I, value::rgb8) debug(box);
200  mln_piter(p_array<P>) p(X);
201  for_all(p)
202  {
203  debug(p) = labeling::internal::random_color(value::rgb8());
204  debug2(p) = true;
205  }
206  io::pbm::save(slice(debug2,0), "debug2-a.ppm");
207 
208  mln_piter(I) pi(cp_ima_.domain());
209  for_all(pi)
210  {
211  debug(pi) = debug(X[cp_ima_(pi)]);
212  debug2(pi) = debug2(X[cp_ima_(pi)]);
213  }
214 
215  io::pbm::save(slice(debug2,0), "debug2-b.ppm");
216  io::ppm::save(slice(debug,0), "debug.ppm");
217  std::cout << "map saved" << std::endl;
218 #endif
219  }
220 
221  mln_site(I) operator()(const mln_site(I)& p) const
222  {
223  return X_[cp_ima_(p)];
224  }
225 
226 
227  // Distance map
228  dmap_t dmap_X_;
229 
230  private:
231  p_array<P> X_;
232  // Closest point image.
233  cp_ima_t cp_ima_;
234 
235  };
236 
237 
239  template <typename P>
241  {
242  typedef mln_image_from_grid(mln_grid(P), P) I;
243  typedef p_array<P> X_t;
244 
245  public:
246 
248  : X_(X)
249  {
250  }
251 
252  mln_site(I) operator()(const vec3d_f& v) const
253  {
254  vec3d_f best_x = X_[0];
255 
256  float best_d = norm::l2_distance(v, best_x);
257  mln_piter(X_t) X_i(X_);
258  for_all(X_i)
259  {
260  vec3d_f X_i_vec = X_i;
261  float d = norm::l2_distance(v, X_i_vec);
262  if (d < best_d)
263  {
264  best_d = d;
265  best_x = X_i_vec;
266  }
267  }
268  return best_x;
269  }
270 
271  private:
272  const p_array<P>& X_;
273  };
274 
275 
276  template <typename P>
277  void
278  draw_last_run(const box3d& box, const p_array<P>& kept,
279  const p_array<P>& removed, const p_array<P>& X,
280  const algebra::quat& qR, const vec3d_f qT)
281  {
282  typedef image3d<value::rgb8> result_t;
283  result_t result(box);
285  ext_result_t ext_result(result, pw::cst(value::rgb8(0,0,0)));
286 
287  data::fill(ext_result, literal::black);
288  data::fill((ext_result | X).rw(), literal::white);
289 
290  mln_piter(p_array<P>) p(kept);
291  for_all(p)
292  ext_result(qR.rotate(p.to_vec()) + qT) = literal::green;
293 
294  mln_piter(p_array<P>) p2(removed);
295  for_all(p2)
296  ext_result(qR.rotate(p2.to_vec()) + qT) = literal::red;
297 
298  io::ppm::save(slice(ext_result,0), "registered-2.ppm");
299  }
300 
301 
302 
303  template <typename P, typename F>
304  float compute_standard_deviation(const p_array<P>& P_,
305  const std::pair<algebra::quat,mln_vec(P)>& pair,
306  const F& closest_point)
307  {
308  accu::rms<vec3d_f,float> e_k_accu;
309 
310  // Standard Deviation
311  float sd;
312  mln_piter(p_array<P>) p(P_);
313  for_all(p)
314  {
315  vec3d_f Pk_i = pair.first.rotate(p) + pair.second;
316  vec3d_f Yk_i = closest_point(Pk_i).to_vec();
317  // yk_i - pk_i
318  e_k_accu.take(Yk_i - Pk_i);
319  }
320 
321  float d = e_k_accu.to_result();
322  sd = std::sqrt(e_k_accu.hook_value_() / P_.nsites() - d * d);
323  return sd;
324  }
325 
326 
327  template <typename P, typename F>
328  void
329  remove_too_far_sites_debug(image3d<value::rgb8>& out, const p_array<P>& P_,
330  const F& closest_point,
331  const std::pair<algebra::quat,mln_vec(P)>& pair,
332  const p_array<P>& X,
333  unsigned r, int d_min, int d_max, unsigned prefix)
334  {
335  unsigned removed = 0;
336  accu::histo<value::int_u8> h;
337  mln_piter(p_array<P>) p(P_);
338  data::fill(out, literal::black);
339  data::fill((out | X).rw(), literal::white);
340 
341  for_all(p)
342  {
343  vec3d_f Pk_i = pair.first.rotate(p) + pair.second;
344  vec3d_f Yk_i = closest_point(Pk_i);
345 
346  int d_i = closest_point.dmap_X_(Pk_i);
347  if (d_i >= d_min && d_i <= d_max)
348  out(Pk_i) = literal::green;
349  else
350  {
351  ++removed;
352  out(Pk_i) = literal::red;
353  }
354  }
355 
356 #ifndef NDEBUG
357  std::ostringstream ss1;
358  ss1 << "histo_" << prefix << r << ".dat";
359  std::cout << h << std::endl;
360 
361  std::ostringstream ss2;
362  ss2 << "out_" << prefix << r << ".ppm";
363  io::ppm::save(mln::slice(out,0), ss2.str());
364 #endif
365  std::cout << "Points removed with the whole set and current d_min/d_max: " << removed << std::endl;
366 
367  }
368 
369 
370  template <typename P, typename F>
371  void
372  compute_distance_criteria(const p_array<P>& P_,
373  const F& closest_point,
374  const std::pair<algebra::quat,mln_vec(P)>& pair,
375  unsigned r, int& d_min, int& d_max)
376  {
377  mln_piter(p_array<P>) p(P_);
378  accu::histo<value::int_u8> h;
379 
380  float sd;
381  {
382  accu::math::sum<float> s, s2;
383  for_all(p)
384  {
385  vec3d_f Pk_i = pair.first.rotate(p.to_vec()) + pair.second;
386  unsigned d_i = closest_point.dmap_X_(Pk_i);
387  h.take(d_i);
388  s.take(d_i);
389  s2.take(d_i * d_i);
390  }
391  float mean = s / P_.nsites();
392  sd = std::sqrt(s2 / P_.nsites() - mean * mean);
393  d_min = int(mean - sd);
394  d_max = int(mean + sd);
395  }
396 
397  std::cout << "Standard deviation = " << sd << std::endl;
398  std::ostringstream ss1;
399  ss1 << "histo_" << r << ".dat";
400  std::cout << h << std::endl;
401  std::cout << "d thresholds = " << d_min << ' ' << d_max << std::endl;
402  }
403 
404  template <typename P, typename F>
405  p_array<P>
406  remove_too_far_sites(image3d<value::rgb8>& out, const p_array<P>& P_,
407  const F& closest_point,
408  const std::pair<algebra::quat,mln_vec(P)>& pair,
409  const p_array<P>& X, p_array<P>& removed_set,
410  unsigned r, int d_min, int d_max,
411  const std::string& method)
412  {
413  p_array<P> tmp;
414  unsigned removed = 0;
415 
416 # ifndef NDEBUG
418  data::fill((out | X).rw(), literal::white);
419 # endif // ! NDEBUG
420 
421  mln_piter(p_array<P>) p(P_);
422  for_all(p)
423  {
424  vec3d_f Pk_i = pair.first.rotate(p.to_vec()) + pair.second;
425  vec3d_f Yk_i = closest_point(Pk_i);
426 
427  int d_i = closest_point.dmap_X_(Pk_i);
428  if (d_i >= d_min && d_i <= d_max)
429  {
430  tmp.append(p);
431  out(Pk_i) = literal::green;
432  }
433  else
434  {
435  ++removed;
436  removed_set.append(p);
437  out(Pk_i) = literal::red;
438  }
439  }
440 
441  {
442  std::ostringstream ss2;
443  ss2 << method << "_" << r << "_removed_sites" << ".cloud";
444  io::cloud::save(removed_set, ss2.str());
445  }
446  {
447  std::ostringstream ss2;
448  ss2 << method << "_" << r << "_kept_sites" << ".cloud";
449  io::cloud::save(tmp, ss2.str());
450  }
451 
452 # ifndef NDEBUG
453  std::ostringstream ss2;
454  ss2 << method << "_" << r << "_removed_sites" << ".ppm";
455  io::ppm::save(mln::slice(out,0), ss2.str());
456 
457  std::cout << "Points removed: " << removed << std::endl;
458 # endif // ! NDEBUG
459  // They are used for debug purpose only.
460  // When NDEBUG is set, they are unused.
461  (void) X;
462  (void) r;
463  (void) method;
464 
465  return tmp;
466  }
467 
468  template <typename P>
469  void
470  display_sites_used_in_icp(image3d<value::rgb8>& out, const p_array<P>& P_sub,
471  const p_array<P>& P_, const p_array<P>& X,
472  unsigned r, const std::string& prefix,
473  const std::pair<algebra::quat,mln_vec(P)>& pair,
474  const std::string& period, const value::rgb8& c)
475  {
477  data::fill((out | X).rw(), literal::white);
478 
479  mln_piter(p_array<P>) p1(P_);
480  for_all(p1)
481  {
482  vec3d_f Pk_i = pair.first.rotate(p1.to_vec()) + pair.second;
483  out(Pk_i) = literal::red;
484  }
485 
486  mln_piter(p_array<P>) p2(P_sub);
487  for_all(p2)
488  {
489  vec3d_f Pk_i = pair.first.rotate(p2.to_vec()) + pair.second;
490  out(Pk_i) = c;
491  }
492 
493  std::ostringstream ss;
494  ss << prefix << "_" << r << "_" << period << ".ppm";
495 
496  io::ppm::save(slice(out,0), ss.str());
497  }
498 
499 
500  template <typename P, typename F>
501  inline
502  float
503  compute_d_k(const p_array<P>& P_,
504  const F& closest_point,
505  const algebra::quat& qR,
506  const algebra::quat& qR_old,
507  const vec3d_f& qT,
508  const vec3d_f& qT_old)
509  {
510  accu::rms<vec3d_f, float> accu;
511  mln_piter(p_array<P>) p(P_);
512  for_all(p)
513  {
514  // yk_i - pk+1_i
515  vec3d_f P_i = p;
516  vec3d_f Pk_i = qR_old.rotate(P_i) + qT_old;
517  vec3d_f Pk_1_i = qR.rotate(P_i) + qT;
518  accu.take(closest_point(Pk_i).to_vec() - Pk_1_i);
519  }
520  return accu.to_result();
521  }
522 
523 
525  template <typename P, typename F>
526  algebra::quat
527  get_rot(const p_array<P>& P_,
528  const vec3d_f& mu_P,
529  const vec3d_f& mu_Yk,
530  const F& closest_point,
531  const algebra::quat& qR,
532  const vec3d_f& qT)
533  {
535  algebra::mat<3u,3u,float> Spx;
536  mln_piter(p_array<P>) p(P_);
537 
538  // FIXME: could we use an accu?
539  for_all(p)
540  {
541  vec3d_f P_i = p;
542  vec3d_f Pk_i = qR.rotate(P_i) + qT;
543  vec3d_f Yk_i = closest_point(Pk_i);
544  Spx += (P_i - mu_P) * (Yk_i - mu_Yk).t();
545  }
546  Spx /= P_.nsites();
547 
548  vec3d_f A;
549  A[0] = Spx(1,2) - Spx(2,1);
550  A[1] = Spx(2,0) - Spx(0,2);
551  A[2] = Spx(0,1) - Spx(1,0);
552 
553  algebra::mat<4u,4u,float> Qk;
554  float t = tr(Spx);
555 
556  Qk(0,0) = t;
557  for (int i = 1; i < 4; ++i)
558  {
559  Qk(i,0) = A[i - 1];
560  Qk(0,i) = A[i - 1];
561  for (int j = 1; j < 4; ++j)
562  if (i == j)
563  Qk(i,j) = 2 * Spx(i - 1,i - 1) - t;
564  }
565 
566  Qk(1,2) = Spx(0,1) + Spx(1,0);
567  Qk(2,1) = Spx(0,1) + Spx(1,0);
568 
569  Qk(1,3) = Spx(0,2) + Spx(2,0);
570  Qk(3,1) = Spx(0,2) + Spx(2,0);
571 
572  Qk(2,3) = Spx(1,2) + Spx(2,1);
573  Qk(3,2) = Spx(1,2) + Spx(2,1);
574 
575  return math::jacobi(Qk);
576  }
577 
578 
579  // Compute mu_Yk, mass center of Yk.
580  template <typename P, typename F>
581  inline
582  vec3d_f
583  get_mu_yk(const p_array<P>& P_,
584  const F& closest_point,
585  const algebra::quat& qR,
586  const vec3d_f& qT,
587  float& e_k)
588  {
589  accu::rms<vec3d_f,float> e_k_accu;
591 
592  mln_piter(p_array<P>) p(P_);
593  for_all(p)
594  {
595  // yk_i - pk_i
596  vec3d_f Pk_i = qR.rotate(p.to_vec()) + qT;
597  vec3d_f Yk_i = closest_point(Pk_i);
598  mu_yk.take(Yk_i);
599  e_k_accu.take(Yk_i - Pk_i);
600  }
601 
602  e_k = e_k_accu.to_result();
603  return mu_yk.to_result();
604  }
605 
606 
607 
609  template <typename P, typename F>
610  inline
611  std::pair<algebra::quat,mln_vec(P)>
612  icp(const p_array<P>& P_,
613  const p_array<P>& X,
614  const F& closest_point,
615  const algebra::quat& initial_rot,
616  const mln_vec(P)& initial_translation)
617  {
618  trace::entering("registration::icp");
619 
620  (void) X;
621  mln_precondition(P::dim == 3);
622  mln_precondition(!P_.is_empty());
623  mln_precondition(!X.is_empty());
624  mln_precondition(!initial_rot.is_null());
625 
626  typedef p_array<P> cloud_t;
627 
629 
630  vec3d_f qT_old, qT = initial_translation;
631  algebra::quat qR_old, qR = initial_rot;
632  float e_k, e_k_old = mln_max(float);
633  float d_k, d_k_old = mln_max(float);
634  unsigned k = 0;
635 
636 # ifndef NDEBUG
637  box3d box = geom::bbox(X);
638  //FIXME: too large?
639  box.enlarge(1, box.nrows() / 2);
640  box.enlarge(2, box.ncols() / 2);
641  image3d<value::rgb8> debug(box);
642  data::fill(debug, literal::black);
643  data::fill((debug | X).rw(), literal::white);
644 # endif
645 
646  do
647  {
648  qT_old = qT;
649  qR_old = qR;
650 
653  // mu_Yk - Pk's mass center.
654  // + Compute error ek = d(Pk,Yk)
655  vec3d_f mu_Yk = get_mu_yk(P_, closest_point, qR_old, qT_old, e_k);
656 
657  // quaternion qR - rotation
658  qR = get_rot(P_, mu_P, mu_Yk, closest_point, qR_old, qT_old);
659  vec3d_f tmp = qR.v();
660 
661  // vector qT - translation
662  qT = mu_Yk - qR.rotate(mu_P);
665 
666  // Distance dk = d(Pk+1, Yk)
667  d_k = compute_d_k(P_, closest_point, qR, qR_old, qT, qT_old);
668 
669 
670 #ifndef NDEBUG
671  image3d<value::rgb8> tmp_ = duplicate(debug);
672  mln_piter(p_array<P>) p_dbg(P_);
673  for_all(p_dbg)
674  tmp_(qR_old.rotate(p_dbg.to_vec()) + qT_old) = literal::green;
675  std::ostringstream ss;
676  ss << "tmp_0";
677  if (k < 10)
678  ss << "0";
679  ss << k << ".ppm";
680  io::ppm::save(mln::slice(tmp_,0), ss.str());
681 #endif
682 
683  std::cout << "e_" << k << "=" << e_k << std::endl;
684  std::cout << "d_" << k << "=" << d_k << std::endl;
685 
686  // Check distance and error according to the related paper.
687  // Disabled because of the following 'if'
688 // mln_assertion(0 <= d_k);
689 // mln_assertion(d_k <= e_k);
690 
691 // mln_assertion(e_k <= d_k_old);
692 // mln_assertion(d_k_old <= e_k_old);
693 
694  // During the first runs, d_k may be higher than e_k.
695  // Hence, there we test k > 3.
696  if (k > 3 && (d_k > e_k || d_k > d_k_old || e_k > e_k_old))
697  {
698  qR = qR_old;
699  qT = qT_old;
700  break;
701  }
702 
703  // Backing up results.
704  d_k_old = d_k;
705  e_k_old = e_k;
706 
707  ++k;
708 
709  } while ((k < 3)
710  || norm::l2((qT - qT_old)) + norm::l2((qR - qR_old).to_vec()) > 1e-3);
711 
712  trace::exiting("registration::icp");
713  return std::make_pair(qR, qT);
714  }
715 
716 
717 # endif // ! MLN_INCLUDE_ONLY
718 
719  } // end of namespace mln::registration
720 
721 } // end of namespace mln
722 
723 #endif // ! MLN_REGISTRATION_ICP_HH