Milena (Olena)  User documentation 2.0a Id
 All Classes Namespaces Functions Variables Typedefs Enumerator Groups Pages
registration.hh
1 // Copyright (C) 2008, 2009 EPITA Research and Development Laboratory (LRDE)
2 //
3 // This file is part of Olena.
4 //
5 // Olena is free software: you can redistribute it and/or modify it under
6 // the terms of the GNU General Public License as published by the Free
7 // Software Foundation, version 2 of the License.
8 //
9 // Olena is distributed in the hope that it will be useful,
10 // but WITHOUT ANY WARRANTY; without even the implied warranty of
11 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12 // General Public License for more details.
13 //
14 // You should have received a copy of the GNU General Public License
15 // along with Olena. If not, see <http://www.gnu.org/licenses/>.
16 //
17 // As a special exception, you may use this file as part of a free
18 // software project without restriction. Specifically, if other files
19 // instantiate templates or use macros or inline functions from this
20 // file, or you compile this file and link it with other files to produce
21 // an executable, this file does not by itself cause the resulting
22 // executable to be covered by the GNU General Public License. This
23 // exception does not however invalidate any other reasons why the
24 // executable file might be covered by the GNU General Public License.
25 
26 #ifndef MLN_REGISTRATION_REGISTRATION_HH
27 # define MLN_REGISTRATION_REGISTRATION_HH
28 
33 
34 # include <mln/core/image/image3d.hh>
35 # include <mln/core/site_set/box.hh>
36 # include <mln/registration/icp.hh>
37 # include <mln/fun/x2x/all.hh>
38 # include <mln/fun/x2p/closest_point.hh>
39 # include <mln/convert/to_p_array.hh>
40 
41 //FIXME: to be removed.
42 # include <mln/util/timer.hh>
43 
44 namespace mln
45 {
46 
47  namespace registration
48  {
49 
50  using namespace mln::fun::x2x;
51 
52 
54  template <typename P>
55  inline
57  registration1(const box<P>& domain,
58  const p_array<P>& P_,
59  const p_array<P>& X);
60 
68  template <typename P>
69  inline
71  registration2(const box<P>& domain,
72  const p_array<P>& P_,
73  const p_array<P>& X);
74 
81  template <typename P>
82  inline
84  registration3(const box<P>& domain,
85  const p_array<P>& P_,
86  const p_array<P>& X);
87 
88 
89 
90 # ifndef MLN_INCLUDE_ONLY
91 
92 
93  namespace internal
94  {
95 
96  template <typename P>
97  inline
98  void
99  registration_tests(const p_array<P>& P_, const p_array<P>& X)
100  {
101  mln_assertion(P_.is_valid());
102  mln_assertion(X.is_valid());
103  mln_assertion(!X.is_empty());
104  mln_assertion(!P_.is_empty());
105 
106  // FIXME: Work only in 3D for now...
107  mln_precondition(P::dim == 3);
108  (void) P_;
109  (void) X;
110  }
111 
112  } // end of namespace mln::registration::internal
113 
114 
115  namespace impl
116  {
117 
118  template <typename P>
119  inline
121  registration1(const box<P>& domain,
122  const p_array<P>& P_,
123  const p_array<P>& X)
124  {
125  trace::entering("mln::registration::registration1");
126 
127 # ifndef NDEBUG
128  util::timer t;
129  t.start();
130 # endif // ! NDEBUG
131 
132  registration::closest_point_with_map<P> closest_point(X, domain);
133 
134  std::pair<algebra::quat,mln_vec(P)> pair = icp(P_, X, closest_point,
135  algebra::quat(1,0,0,0),
136  literal::zero);
137 # ifndef NDEBUG
138  std::cout << "icp = " << t << std::endl;
139 # endif // ! NDEBUG
140 
141  typedef rotation<3u,float> rot_t;
142  rot_t tqR(pair.first);
143  typedef translation<3u,float> trans_t;
144  trans_t tqT(pair.second);
145  composed<trans_t, rot_t> result(tqT, tqR);
146 
147  trace::exiting("mln::registration::registration1");
148 
149  return result;
150  }
151 
152 
153  template <typename P>
154  inline
156  registration2(const box<P>& domain,
157  const p_array<P>& P_,
158  const p_array<P>& X)
159  {
160  trace::entering("mln::registration::registration2");
161 
162  // Used for debug.
163  std::string method = "registration2";
164 
165  registration::closest_point_with_map<P> closest_point(X, domain);
166 
167 # ifndef NDEBUG
168  util::timer t;
169  t.start();
170 # endif // ! NDEBUG
171 
172  // P_bak is shuffled.
173  p_array<P> P_bak = P_;
174 
175  unsigned r = 0;
176  std::pair<algebra::quat,mln_vec(P)> pair;
177  pair.first = algebra::quat(1,0,0,0);
178  pair.second = literal::zero;
179 
180  // Used for debug.
181  image3d<value::rgb8> out(domain);
182 
183  p_array<P> removed_set;
184 
185  do
186  {
187 
188 # ifndef NDEBUG
189  std::cout << std::endl << std::endl << "==== New run - " << r << std::endl;
190 # endif // ! NDEBUG
191 
192  pair = icp(P_bak, X, closest_point,
193  pair.first,
194  pair.second);
195 
196 # ifndef NDEBUG
197  display_sites_used_in_icp(out, P_bak, P_, X, r, method, pair,
198  "final", literal::blue);
199 # endif // ! NDEBUG
200 
201  int d_min, d_max;
202  compute_distance_criteria(P_bak, closest_point, pair, r, d_min, d_max);
203 
204  P_bak = remove_too_far_sites(out, P_bak,
205  closest_point, pair, X, removed_set,
206  r, d_min, d_max, method);
207 
208 # ifndef NDEBUG
209  display_sites_used_in_icp(out, P_bak, P_, X, r, method, pair,
210  "schanges", literal::green);
211  std::cout << "==== End of run" << std::endl;
212 # endif
213 
214  ++r;
215 
216  } while (r < 10);
217 
218 # ifndef NDEBUG
219  std::cout << "icp = " << t << std::endl;
220  draw_last_run(domain, P_bak, removed_set, X, pair.first, pair.second);
221 # endif
222 
223  typedef rotation<3u,float> rot_t;
224  rot_t tqR(pair.first);
225  typedef translation<3u,float> trans_t;
226  trans_t tqT(pair.second);
227  composed<trans_t,rot_t> result(tqT, tqR);
228 
229  trace::exiting("mln::registration::registration2");
230 
231  return result;
232  }
233 
234 
235  template <typename P>
236  inline
238  registration3(const box<P>& domain,
239  const p_array<P>& P_,
240  const p_array<P>& X)
241  {
242  trace::entering("mln::registration::registration3");
243 
244  registration::closest_point_with_map<P> closest_point(X, domain);
245 
246  // Used for debug.
247  std::string method = "registration3";
248 
249 # ifndef NDEBUG
250  util::timer t;
251  t.start();
252 # endif // ! NDEBUG
253 
254  // P_bak is shuffled.
255  p_array<P> P_bak = P_;
256 
257  unsigned r = 0;
258  std::pair<algebra::quat,mln_vec(P)> pair;
259  pair.first = algebra::quat(1,0,0,0);
260  pair.second = literal::zero;
261 
262  // Used for debug.
263  image3d<value::rgb8> out(domain);
264 
265  p_array<P> removed_set;
266 
267  do
268  {
269 # ifndef NDEBUG
270  std::cout << std::endl << std::endl << "==== New run - "
271  << r << std::endl;
272 # endif // ! NDEBUG
273 
274  pair = icp(P_bak, X, closest_point,
275  pair.first,
276  pair.second);
277 
278 # ifndef NDEBUG
279  display_sites_used_in_icp(out, P_bak, P_, X, r, method, pair,
280  "final", literal::blue);
281 # endif // ! NDEBUG
282 
283  int d_min, d_max;
284  compute_distance_criteria(P_bak, closest_point, pair, r, d_min, d_max);
285 
286  P_bak = remove_too_far_sites(out, P_,
287  closest_point, pair, X, removed_set,
288  r, d_min, d_max, method);
289 
290 # ifndef NDEBUG
291  display_sites_used_in_icp(out, P_bak, P_, X, r, method, pair,
292  "schanges", literal::green);
293  std::cout << "==== End of run" << std::endl;
294 # endif // ! NDEBUG
295 
296  ++r;
297 
298  } while (r < 10);
299 
300 # ifndef NDEBUG
301  std::cout << "icp = " << t << std::endl;
302  draw_last_run(domain, P_bak, removed_set, X, pair.first, pair.second);
303 # endif // ! NDEBUG
304 
305  typedef rotation<3u,float> rot_t;
306  rot_t tqR(pair.first);
307  typedef translation<3u,float> trans_t;
308  trans_t tqT(pair.second);
309  composed<trans_t,rot_t> result(tqT, tqR);
310 
311  trace::exiting("mln::registration::registration3");
312 
313  return result;
314  }
315 
316  } // end of namespace mln::registration::impl
317 
318 
319 
320  // Facade
321 
322  template <typename P>
323  inline
325  registration1(const box<P>& domain,
326  const p_array<P>& cloud,
327  const p_array<P>& surface)
328  {
329  trace::entering("registration::registration1");
330 
331  internal::registration_tests(cloud, surface);
332 
334  qk = impl::registration1(domain, cloud, surface);
335 
336  trace::exiting("registration::registration1");
337 
338  return qk;
339  }
340 
341 
342  template <typename P>
343  inline
345  registration2(const box<P>& domain,
346  const p_array<P>& cloud,
347  const p_array<P>& surface)
348  {
349  trace::entering("registration::registration2");
350 
351  internal::registration_tests(cloud, surface);
352 
354  qk = impl::registration2(domain, cloud, surface);
355 
356  trace::exiting("registration::registration2");
357 
358  return qk;
359  }
360 
361 
362  template <typename P>
363  inline
365  registration3(const box<P>& domain,
366  const p_array<P>& cloud,
367  const p_array<P>& surface)
368  {
369  trace::entering("registration::registration3");
370 
371  internal::registration_tests(cloud, surface);
372 
374  qk = impl::registration3(domain, cloud, surface);
375 
376  trace::exiting("registration::registration3");
377 
378  return qk;
379  }
380 
381 
382 # endif // ! MLN_INCLUDE_ONLY
383 
384 
385  } // end of namespace mln::registration
386 
387 
388 } // end of namespace mln
389 
390 
391 #endif // ! MLN_REGISTRATION_REGISTRATION_HH