$extrastylesheet
Olena  User documentation 2.1
An Image Processing Platform
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
icp.hh
1 // Copyright (C) 2008, 2009, 2010, 2011, 2013 EPITA Research and
2 // Development 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 
113  template <typename P, typename F>
114  std::pair<algebra::quat,mln_vec(P)>
115  icp(const p_array<P>& P_,
116  const p_array<P>& X,
117  const F& closest_point,
118  const algebra::quat& initial_rot,
119  const mln_vec(P)& initial_translation);
120 
121 
132  template <typename P, typename F>
133  composed< translation<P::dim,float>,rotation<P::dim,float> >
134  icp(const p_array<P>& P_,
135  const p_array<P>& X,
136  const F& closest_point);
137 
138 
139 # ifndef MLN_INCLUDE_ONLY
140 
141 
143  template <typename P>
144  class closest_point_with_map
145  {
146  typedef mln_image_from_grid(mln_grid(P), P) I;
147  typedef mln_ch_value(I, unsigned) cp_ima_t;
148  typedef mln_ch_value(I,value::int_u16) dmap_t;
149 
150  public:
151 
152  closest_point_with_map(const p_array<P>& X)
153  {
154  box3d box = geom::bbox(X);
155  box.enlarge(0, box.nslis());
156  box.enlarge(1, box.nrows());
157  box.enlarge(2, box.ncols());
158 
159  mln_postcondition(box.is_valid());
160 
161 #ifndef NDEBUG
162  std::cout << "Map image defined on " << box << std::endl;
163 #endif // NDEBUG
164 
165  X_ = X;
166  init(X, box);
167  }
168 
169  closest_point_with_map(const p_array<P>& X, const box<P>& box)
170  {
171  X_ = X;
172  init(X, box);
173  }
174 
175  void init(const p_array<P>& X, const box<P>& box)
176  {
177  typedef mln_ch_value(I, bool) model_t;
178  model_t model(box);
179  data::fill(model, false);
180  data::fill((model | X).rw(), true);
181 
182 
183  typedef util::couple<mln_ch_value(model_t, value::int_u16),
184  mln_ch_value(model_t, unsigned)> couple_t;
185  couple_t cpl = transform::distance_and_closest_point_geodesic(X, box,
186  c6(),
187  mln_max(value::int_u16));
188 
189  dmap_X_ = cpl.first();
190  cp_ima_ = cpl.second();
191 
192  mln_postcondition(cp_ima_.is_valid());
193  mln_postcondition(cp_ima_.domain().is_valid());
194 
195 #ifndef NDEBUG
196  std::cout << "pmin = " << cp_ima_.domain().pmin() << std::endl;;
197  std::cout << "pmax = " << cp_ima_.domain().pmax() << std::endl;;
198 
199  mln_ch_value(I, bool) debug2(box);
200  data::fill(debug2, false);
201  mln_ch_value(I, value::rgb8) debug(box);
202  mln_piter(p_array<P>) p(X);
203  for_all(p)
204  {
205  debug(p) = labeling::internal::random_color(value::rgb8());
206  debug2(p) = true;
207  }
208  io::pbm::save(slice(debug2,0), "debug2-a.ppm");
209 
210  mln_piter(I) pi(cp_ima_.domain());
211  for_all(pi)
212  {
213  debug(pi) = debug(X[cp_ima_(pi)]);
214  debug2(pi) = debug2(X[cp_ima_(pi)]);
215  }
216 
217  io::pbm::save(slice(debug2,0), "debug2-b.ppm");
218  io::ppm::save(slice(debug,0), "debug.ppm");
219  std::cout << "map saved" << std::endl;
220 #endif
221  }
222 
223  mln_site(I) operator()(const mln_site(I)& p) const
224  {
225  return X_[cp_ima_(p)];
226  }
227 
228 
229  // Distance map
230  dmap_t dmap_X_;
231 
232  private:
233  p_array<P> X_;
234  // Closest point image.
235  cp_ima_t cp_ima_;
236 
237  };
238 
239 
241  template <typename P>
242  class closest_point_basic
243  {
244  typedef mln_image_from_grid(mln_grid(P), P) I;
245  typedef p_array<P> X_t;
246 
247  public:
248 
249  closest_point_basic(const p_array<P>& X)
250  : X_(X)
251  {
252  }
253 
254  mln_site(I) operator()(const vec3d_f& v) const
255  {
256  vec3d_f best_x = X_[0];
257 
258  float best_d = norm::l2_distance(v, best_x);
259  mln_piter(X_t) X_i(X_);
260  for_all(X_i)
261  {
262  vec3d_f X_i_vec = X_i;
263  float d = norm::l2_distance(v, X_i_vec);
264  if (d < best_d)
265  {
266  best_d = d;
267  best_x = X_i_vec;
268  }
269  }
270  return best_x;
271  }
272 
273  private:
274  const p_array<P>& X_;
275  };
276 
277 
278  template <typename P>
279  void
280  draw_last_run(const box3d& box, const p_array<P>& kept,
281  const p_array<P>& removed, const p_array<P>& X,
282  const algebra::quat& qR, const vec3d_f qT)
283  {
284  typedef image3d<value::rgb8> result_t;
285  result_t result(box);
286  typedef extension_fun<result_t,pw::cst_<mln_value(result_t)> > ext_result_t;
287  ext_result_t ext_result(result, pw::cst(value::rgb8(0,0,0)));
288 
289  data::fill(ext_result, literal::black);
290  data::fill((ext_result | X).rw(), literal::white);
291 
292  mln_piter(p_array<P>) p(kept);
293  for_all(p)
294  ext_result(qR.rotate(p.to_vec()) + qT) = literal::green;
295 
296  mln_piter(p_array<P>) p2(removed);
297  for_all(p2)
298  ext_result(qR.rotate(p2.to_vec()) + qT) = literal::red;
299 
300  io::ppm::save(slice(ext_result,0), "registered-2.ppm");
301  }
302 
303 
304 
305  template <typename P, typename F>
306  float compute_standard_deviation(const p_array<P>& P_,
307  const std::pair<algebra::quat,mln_vec(P)>& pair,
308  const F& closest_point)
309  {
310  accu::rms<vec3d_f,float> e_k_accu;
311 
312  // Standard Deviation
313  float sd;
314  mln_piter(p_array<P>) p(P_);
315  for_all(p)
316  {
317  vec3d_f Pk_i = pair.first.rotate(p) + pair.second;
318  vec3d_f Yk_i = closest_point(Pk_i).to_vec();
319  // yk_i - pk_i
320  e_k_accu.take(Yk_i - Pk_i);
321  }
322 
323  float d = e_k_accu.to_result();
324  sd = std::sqrt(e_k_accu.hook_value_() / P_.nsites() - d * d);
325  return sd;
326  }
327 
328 
329  template <typename P, typename F>
330  void
331  remove_too_far_sites_debug(image3d<value::rgb8>& out, const p_array<P>& P_,
332  const F& closest_point,
333  const std::pair<algebra::quat,mln_vec(P)>& pair,
334  const p_array<P>& X,
335  unsigned r, int d_min, int d_max, unsigned prefix)
336  {
337  unsigned removed = 0;
338  accu::histo<value::int_u8> h;
339  mln_piter(p_array<P>) p(P_);
340  data::fill(out, literal::black);
341  data::fill((out | X).rw(), literal::white);
342 
343  for_all(p)
344  {
345  vec3d_f Pk_i = pair.first.rotate(p) + pair.second;
346  // FIXME: Unused variable; remove?
347 #if 0
348  vec3d_f Yk_i = closest_point(Pk_i);
349 #endif
350 
351  int d_i = closest_point.dmap_X_(Pk_i);
352  if (d_i >= d_min && d_i <= d_max)
353  out(Pk_i) = literal::green;
354  else
355  {
356  ++removed;
357  out(Pk_i) = literal::red;
358  }
359  }
360 
361 #ifndef NDEBUG
362  std::ostringstream ss1;
363  ss1 << "histo_" << prefix << r << ".dat";
364  std::cout << h << std::endl;
365 
366  std::ostringstream ss2;
367  ss2 << "out_" << prefix << r << ".ppm";
368  io::ppm::save(mln::slice(out,0), ss2.str());
369 
370  std::cout
371  << "Points removed with the whole set and current d_min/d_max: "
372  << removed << std::endl;
373 #endif
374  }
375 
376 
377  /* FIXME: Unused argument `r'; remove? */
378  template <typename P, typename F>
379  void
380  compute_distance_criteria(const p_array<P>& P_,
381  const F& closest_point,
382  const std::pair<algebra::quat,mln_vec(P)>& pair,
383  unsigned /* r */, int& d_min, int& d_max)
384  {
385  mln_piter(p_array<P>) p(P_);
386  accu::histo<value::int_u8> h;
387 
388  float sd;
389  {
390  accu::math::sum<float> s, s2;
391  for_all(p)
392  {
393  vec3d_f Pk_i = pair.first.rotate(p.to_vec()) + pair.second;
394  unsigned d_i = closest_point.dmap_X_(Pk_i);
395  h.take(d_i);
396  s.take(d_i);
397  s2.take(d_i * d_i);
398  }
399  float mean = s / P_.nsites();
400  sd = std::sqrt(s2 / P_.nsites() - mean * mean);
401  d_min = int(mean - sd);
402  d_max = int(mean + sd);
403  }
404 
405 #ifndef NDEBUG
406  std::cout << "Standard deviation = " << sd << std::endl;
407  std::cout << h << std::endl;
408  std::cout << "d thresholds = " << d_min << ' ' << d_max << std::endl;
409 #endif // ! NDEBUG
410  }
411 
412  template <typename P, typename F>
413  p_array<P>
414  remove_too_far_sites(image3d<value::rgb8>& out, const p_array<P>& P_,
415  const F& closest_point,
416  const std::pair<algebra::quat,mln_vec(P)>& pair,
417  const p_array<P>& X, p_array<P>& removed_set,
418  unsigned r, int d_min, int d_max,
419  const std::string& method)
420  {
421  p_array<P> tmp;
422  unsigned removed = 0;
423 
424 # ifndef NDEBUG
426  data::fill((out | X).rw(), literal::white);
427 # endif // ! NDEBUG
428 
429  mln_piter(p_array<P>) p(P_);
430  for_all(p)
431  {
432  vec3d_f Pk_i = pair.first.rotate(p.to_vec()) + pair.second;
433  //vec3d_f Yk_i = closest_point(Pk_i);
434 
435  int d_i = closest_point.dmap_X_(Pk_i);
436  if (d_i >= d_min && d_i <= d_max)
437  {
438  tmp.append(p);
439  out(Pk_i) = literal::green;
440  }
441  else
442  {
443  ++removed;
444  removed_set.append(p);
445  out(Pk_i) = literal::red;
446  }
447  }
448 
449 # ifndef NDEBUG
450  {
451  std::ostringstream ss2;
452  ss2 << method << "_" << r << "_removed_sites" << ".cloud";
453  io::cloud::save(removed_set, ss2.str());
454  }
455  {
456  std::ostringstream ss2;
457  ss2 << method << "_" << r << "_kept_sites" << ".cloud";
458  io::cloud::save(tmp, ss2.str());
459  }
460 
461  std::ostringstream ss2;
462  ss2 << method << "_" << r << "_removed_sites" << ".ppm";
463  io::ppm::save(mln::slice(out,0), ss2.str());
464 
465  std::cout << "Points removed: " << removed << std::endl;
466 # endif // ! NDEBUG
467  // They are used for debug purpose only.
468  // When NDEBUG is set, they are unused.
469  (void) X;
470  (void) r;
471  (void) method;
472 
473  return tmp;
474  }
475 
476  template <typename P>
477  void
478  display_sites_used_in_icp(image3d<value::rgb8>& out, const p_array<P>& P_sub,
479  const p_array<P>& P_, const p_array<P>& X,
480  unsigned r, const std::string& prefix,
481  const std::pair<algebra::quat,mln_vec(P)>& pair,
482  const std::string& period, const value::rgb8& c)
483  {
485  data::fill((out | X).rw(), literal::white);
486 
487  mln_piter(p_array<P>) p1(P_);
488  for_all(p1)
489  {
490  vec3d_f Pk_i = pair.first.rotate(p1.to_vec()) + pair.second;
491  out(Pk_i) = literal::red;
492  }
493 
494  mln_piter(p_array<P>) p2(P_sub);
495  for_all(p2)
496  {
497  vec3d_f Pk_i = pair.first.rotate(p2.to_vec()) + pair.second;
498  out(Pk_i) = c;
499  }
500 
501  std::ostringstream ss;
502  ss << prefix << "_" << r << "_" << period << ".ppm";
503 
504  io::ppm::save(slice(out,0), ss.str());
505  }
506 
507 
508  template <typename P, typename F>
509  inline
510  float
511  compute_d_k(const p_array<P>& P_,
512  const F& closest_point,
513  const algebra::quat& qR,
514  const algebra::quat& qR_old,
515  const vec3d_f& qT,
516  const vec3d_f& qT_old)
517  {
518  accu::rms<vec3d_f, float> accu;
519  mln_piter(p_array<P>) p(P_);
520  for_all(p)
521  {
522  // yk_i - pk+1_i
523  vec3d_f P_i = p;
524  vec3d_f Pk_i = qR_old.rotate(P_i) + qT_old;
525  vec3d_f Pk_1_i = qR.rotate(P_i) + qT;
526  accu.take(closest_point(Pk_i).to_vec() - Pk_1_i);
527  }
528  return accu.to_result();
529  }
530 
531 
533  template <typename P, typename F>
534  algebra::quat
535  get_rot(const p_array<P>& P_,
536  const vec3d_f& mu_P,
537  const vec3d_f& mu_Yk,
538  const F& closest_point,
539  const algebra::quat& qR,
540  const vec3d_f& qT)
541  {
543  algebra::mat<3u,3u,float> Spx(literal::zero);
544  mln_piter(p_array<P>) p(P_);
545 
546  // FIXME: could we use an accu?
547  for_all(p)
548  {
549  vec3d_f P_i = p;
550  vec3d_f Pk_i = qR.rotate(P_i) + qT;
551  vec3d_f Yk_i = closest_point(Pk_i);
552  Spx += (P_i - mu_P) * (Yk_i - mu_Yk).t();
553  }
554  Spx /= P_.nsites();
555 
556  vec3d_f A;
557  A[0] = Spx(1,2) - Spx(2,1);
558  A[1] = Spx(2,0) - Spx(0,2);
559  A[2] = Spx(0,1) - Spx(1,0);
560 
561  algebra::mat<4u,4u,float> Qk;
562  float t = tr(Spx);
563 
564  Qk(0,0) = t;
565  for (int i = 1; i < 4; ++i)
566  {
567  Qk(i,0) = A[i - 1];
568  Qk(0,i) = A[i - 1];
569  for (int j = 1; j < 4; ++j)
570  if (i == j)
571  Qk(i,j) = 2 * Spx(i - 1,i - 1) - t;
572  }
573 
574  Qk(1,2) = Spx(0,1) + Spx(1,0);
575  Qk(2,1) = Spx(0,1) + Spx(1,0);
576 
577  Qk(1,3) = Spx(0,2) + Spx(2,0);
578  Qk(3,1) = Spx(0,2) + Spx(2,0);
579 
580  Qk(2,3) = Spx(1,2) + Spx(2,1);
581  Qk(3,2) = Spx(1,2) + Spx(2,1);
582 
583  return math::jacobi(Qk);
584  }
585 
586 
587  // Compute mu_Yk, mass center of Yk.
588  template <typename P, typename F>
589  inline
590  vec3d_f
591  get_mu_yk(const p_array<P>& P_,
592  const F& closest_point,
593  const algebra::quat& qR,
594  const vec3d_f& qT,
595  float& e_k)
596  {
597  accu::rms<vec3d_f,float> e_k_accu;
598  accu::center<P,vec3d_f> mu_yk;
599 
600  mln_piter(p_array<P>) p(P_);
601  for_all(p)
602  {
603  // yk_i - pk_i
604  vec3d_f Pk_i = qR.rotate(p.to_vec()) + qT;
605  vec3d_f Yk_i = closest_point(Pk_i);
606  mu_yk.take(Yk_i);
607  e_k_accu.take(Yk_i - Pk_i);
608  }
609 
610  e_k = e_k_accu.to_result();
611  return mu_yk.to_result();
612  }
613 
614 
615 
617  template <typename P, typename F>
618  inline
619  std::pair<algebra::quat,mln_vec(P)>
620  icp(const p_array<P>& P_,
621  const p_array<P>& X,
622  const F& closest_point,
623  const algebra::quat& initial_rot,
624  const mln_vec(P)& initial_translation)
625  {
626  mln_trace("registration::icp");
627 
628  (void) X;
629  mln_precondition(P::dim == 3);
630  mln_precondition(!P_.is_empty());
631  mln_precondition(!X.is_empty());
632  mln_precondition(!initial_rot.is_null());
633 
634  typedef p_array<P> cloud_t;
635 
636  vec3d_f mu_P = set::compute(accu::center<P,vec3d_f>(), P_);
637 
638  vec3d_f qT_old, qT = initial_translation;
639  algebra::quat qR_old, qR = initial_rot;
640  float e_k, e_k_old = mln_max(float);
641  float d_k, d_k_old = mln_max(float);
642  unsigned k = 0;
643 
644 # ifndef NDEBUG
645  box3d box = geom::bbox(X);
646  //FIXME: too large?
647  box.enlarge(1, box.nrows() / 2);
648  box.enlarge(2, box.ncols() / 2);
649  image3d<value::rgb8> debug(box);
650  data::fill(debug, literal::black);
651  data::fill((debug | X).rw(), literal::white);
652 # endif
653 
654  do
655  {
656  qT_old = qT;
657  qR_old = qR;
658 
661  // mu_Yk - Pk's mass center.
662  // + Compute error ek = d(Pk,Yk)
663  vec3d_f mu_Yk = get_mu_yk(P_, closest_point, qR_old, qT_old, e_k);
664 
665  // quaternion qR - rotation
666  qR = get_rot(P_, mu_P, mu_Yk, closest_point, qR_old, qT_old);
667 
668  // vector qT - translation
669  qT = mu_Yk - qR.rotate(mu_P);
672 
673  // Distance dk = d(Pk+1, Yk)
674  d_k = compute_d_k(P_, closest_point, qR, qR_old, qT, qT_old);
675 
676 
677 #ifndef NDEBUG
678  image3d<value::rgb8> tmp_ = duplicate(debug);
679  mln_piter(p_array<P>) p_dbg(P_);
680  for_all(p_dbg)
681  tmp_(qR_old.rotate(p_dbg.to_vec()) + qT_old) = literal::green;
682  std::ostringstream ss;
683  ss << "tmp_0";
684  if (k < 10)
685  ss << "0";
686  ss << k << ".ppm";
687  io::ppm::save(mln::slice(tmp_,0), ss.str());
688 
689  std::cout << "e_" << k << "=" << e_k << std::endl;
690  std::cout << "d_" << k << "=" << d_k << std::endl;
691 #endif // ! NDEBUG
692 
693  // Check distance and error according to the related paper.
694  // Disabled because of the following 'if'
695 // mln_assertion(0 <= d_k);
696 // mln_assertion(d_k <= e_k);
697 
698 // mln_assertion(e_k <= d_k_old);
699 // mln_assertion(d_k_old <= e_k_old);
700 
701  // During the first runs, d_k may be higher than e_k.
702  // Hence, there we test k > 3.
703  if (k > 3 && (d_k > e_k || d_k > d_k_old || e_k > e_k_old))
704  {
705  qR = qR_old;
706  qT = qT_old;
707  break;
708  }
709 
710  // Backing up results.
711  d_k_old = d_k;
712  e_k_old = e_k;
713 
714  ++k;
715 
716  } while ((k < 3)
717  || norm::l2((qT - qT_old)) + norm::l2((qR - qR_old).to_vec()) > 1e-3);
718 
719  return std::make_pair(qR, qT);
720  }
721 
722 
723 # endif // ! MLN_INCLUDE_ONLY
724 
725  } // end of namespace mln::registration
726 
727 } // end of namespace mln
728 
729 #endif // ! MLN_REGISTRATION_ICP_HH