27 #ifndef MLN_REGISTRATION_ICP_HH
28 # define MLN_REGISTRATION_ICP_HH
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>
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>
55 # include <mln/core/alias/neighb3d.hh>
57 # include <mln/transform/distance_and_closest_point_geodesic.hh>
58 # include <mln/canvas/distance_geodesic.hh>
59 # include <mln/pw/all.hh>
61 # include <mln/io/ppm/save.hh>
62 # include <mln/io/pbm/save.hh>
64 # include <mln/labeling/colorize.hh>
65 # include <mln/debug/histo.hh>
67 # include <mln/accu/histo.hh>
68 # include <mln/accu/math/sum.hh>
70 # include <mln/value/int_u16.hh>
72 # include <mln/literal/black.hh>
73 # include <mln/literal/white.hh>
74 # include <mln/literal/colors.hh>
76 # include <mln/util/timer.hh>
78 # include <mln/io/cloud/save.hh>
84 namespace registration
89 using namespace fun::x2x;
113 template <
typename P,
typename F>
114 std::pair<algebra::quat,mln_vec(P)>
115 icp(
const p_array<P>& P_,
117 const F& closest_point,
118 const algebra::quat& initial_rot,
119 const mln_vec(P)& initial_translation);
132 template <
typename P,
typename F>
133 composed< translation<P::dim,float>,rotation<P::dim,float> >
134 icp(
const p_array<P>& P_,
136 const F& closest_point);
139 # ifndef MLN_INCLUDE_ONLY
143 template <
typename P>
144 class closest_point_with_map
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;
152 closest_point_with_map(const p_array<P>& X)
155 box.enlarge(0, box.nslis());
156 box.enlarge(1, box.nrows());
157 box.enlarge(2, box.ncols());
159 mln_postcondition(box.is_valid());
162 std::cout <<
"Map image defined on " << box << std::endl;
169 closest_point_with_map(
const p_array<P>& X,
const box<P>& box)
175 void init(
const p_array<P>& X,
const box<P>& box)
177 typedef mln_ch_value(I,
bool) model_t;
183 typedef util::couple<mln_ch_value(model_t,
value::int_u16),
184 mln_ch_value(model_t,
unsigned)> couple_t;
187 mln_max(
value::int_u16));
189 dmap_X_ = cpl.first();
190 cp_ima_ = cpl.second();
192 mln_postcondition(cp_ima_.is_valid());
193 mln_postcondition(cp_ima_.domain().is_valid());
196 std::cout <<
"pmin = " << cp_ima_.domain().pmin() << std::endl;;
197 std::cout <<
"pmax = " << cp_ima_.domain().pmax() << std::endl;;
199 mln_ch_value(I,
bool) debug2(box);
201 mln_ch_value(I,
value::rgb8) debug(box);
202 mln_piter(p_array<P>)
p(X);
205 debug(p) = labeling::internal::random_color(value::rgb8());
210 mln_piter(I)
pi(cp_ima_.domain());
213 debug(pi) = debug(X[cp_ima_(pi)]);
214 debug2(pi) = debug2(X[cp_ima_(pi)]);
219 std::cout <<
"map saved" << std::endl;
223 mln_site(I) operator()(const mln_site(I)& p)
const
225 return X_[cp_ima_(p)];
241 template <
typename P>
242 class closest_point_basic
244 typedef mln_image_from_grid(mln_grid(P), P) I;
245 typedef p_array<P> X_t;
249 closest_point_basic(const p_array<P>& X)
254 mln_site(I) operator()(const vec3d_f& v)
const
256 vec3d_f best_x = X_[0];
259 mln_piter(X_t) X_i(X_);
262 vec3d_f X_i_vec = X_i;
274 const p_array<P>& X_;
278 template <
typename P>
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)
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)));
292 mln_piter(p_array<P>) p(kept);
294 ext_result(qR.
rotate(p.to_vec()) + qT) = literal::green;
296 mln_piter(p_array<P>) p2(removed);
298 ext_result(qR.
rotate(p2.to_vec()) + qT) = literal::red;
300 io::ppm::
save(
slice(ext_result,0), "registered-2.ppm");
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)
310 accu::rms<vec3d_f,float> e_k_accu;
314 mln_piter(p_array<P>) p(P_);
317 vec3d_f Pk_i = pair.first.rotate(p) + pair.second;
318 vec3d_f Yk_i = closest_point(Pk_i).to_vec();
320 e_k_accu.take(Yk_i - Pk_i);
323 float d = e_k_accu.to_result();
324 sd =
std::sqrt(e_k_accu.hook_value_() / P_.nsites() - d * d);
329 template <
typename P,
typename F>
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,
335 unsigned r,
int d_min,
int d_max,
unsigned prefix)
337 unsigned removed = 0;
338 accu::histo<value::int_u8> h;
339 mln_piter(p_array<P>) p(P_);
341 data::
fill((out | X).rw(), literal::white);
345 vec3d_f Pk_i = pair.first.rotate(p) + pair.second;
348 vec3d_f Yk_i = closest_point(Pk_i);
351 int d_i = closest_point.dmap_X_(Pk_i);
352 if (d_i >= d_min && d_i <= d_max)
362 std::ostringstream ss1;
363 ss1 <<
"histo_" << prefix << r <<
".dat";
364 std::cout << h << std::endl;
366 std::ostringstream ss2;
367 ss2 <<
"out_" << prefix << r <<
".ppm";
371 <<
"Points removed with the whole set and current d_min/d_max: "
372 << removed << std::endl;
378 template <
typename P,
typename F>
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 ,
int& d_min,
int& d_max)
385 mln_piter(p_array<P>) p(P_);
390 accu::math::sum<float> s, s2;
393 vec3d_f Pk_i = pair.first.rotate(p.to_vec()) + pair.second;
394 unsigned d_i = closest_point.dmap_X_(Pk_i);
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);
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;
412 template <
typename P,
typename F>
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)
422 unsigned removed = 0;
429 mln_piter(p_array<P>) p(P_);
432 vec3d_f Pk_i = pair.first.rotate(p.to_vec()) + pair.second;
435 int d_i = closest_point.dmap_X_(Pk_i);
436 if (d_i >= d_min && d_i <= d_max)
444 removed_set.append(p);
451 std::ostringstream ss2;
452 ss2 << method <<
"_" << r <<
"_removed_sites" <<
".cloud";
456 std::ostringstream ss2;
457 ss2 << method <<
"_" << r <<
"_kept_sites" <<
".cloud";
461 std::ostringstream ss2;
462 ss2 << method <<
"_" << r <<
"_removed_sites" <<
".ppm";
465 std::cout <<
"Points removed: " << removed << std::endl;
476 template <
typename P>
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)
487 mln_piter(p_array<P>) p1(P_);
490 vec3d_f Pk_i = pair.first.rotate(p1.to_vec()) + pair.second;
494 mln_piter(p_array<P>) p2(P_sub);
497 vec3d_f Pk_i = pair.first.rotate(p2.to_vec()) + pair.second;
501 std::ostringstream ss;
502 ss << prefix <<
"_" << r <<
"_" << period <<
".ppm";
508 template <
typename P,
typename F>
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,
516 const vec3d_f& qT_old)
518 accu::rms<vec3d_f, float> accu;
519 mln_piter(p_array<P>) p(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);
528 return accu.to_result();
533 template <
typename P,
typename F>
535 get_rot(
const p_array<P>& P_,
537 const vec3d_f& mu_Yk,
538 const F& closest_point,
539 const algebra::quat& qR,
544 mln_piter(p_array<P>) p(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();
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);
561 algebra::mat<4u,4u,float> Qk;
565 for (
int i = 1; i < 4; ++i)
569 for (
int j = 1; j < 4; ++j)
571 Qk(i,j) = 2 * Spx(i - 1,i - 1) - t;
574 Qk(1,2) = Spx(0,1) + Spx(1,0);
575 Qk(2,1) = Spx(0,1) + Spx(1,0);
577 Qk(1,3) = Spx(0,2) + Spx(2,0);
578 Qk(3,1) = Spx(0,2) + Spx(2,0);
580 Qk(2,3) = Spx(1,2) + Spx(2,1);
581 Qk(3,2) = Spx(1,2) + Spx(2,1);
588 template <
typename P,
typename F>
591 get_mu_yk(
const p_array<P>& P_,
592 const F& closest_point,
593 const algebra::quat& qR,
597 accu::rms<vec3d_f,float> e_k_accu;
598 accu::center<P,vec3d_f> mu_yk;
600 mln_piter(p_array<P>) p(P_);
604 vec3d_f Pk_i = qR.rotate(p.to_vec()) + qT;
605 vec3d_f Yk_i = closest_point(Pk_i);
607 e_k_accu.take(Yk_i - Pk_i);
610 e_k = e_k_accu.to_result();
611 return mu_yk.to_result();
617 template <
typename P,
typename F>
619 std::pair<algebra::quat,mln_vec(P)>
620 icp(
const p_array<P>& P_,
622 const F& closest_point,
623 const algebra::quat& initial_rot,
624 const mln_vec(P)& initial_translation)
626 mln_trace(
"registration::icp");
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());
634 typedef p_array<P> cloud_t;
636 vec3d_f mu_P =
set::compute(accu::center<P,vec3d_f>(), P_);
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);
647 box.enlarge(1, box.nrows() / 2);
648 box.enlarge(2, box.ncols() / 2);
649 image3d<value::rgb8> debug(box);
663 vec3d_f mu_Yk = get_mu_yk(P_, closest_point, qR_old, qT_old, e_k);
666 qR = get_rot(P_, mu_P, mu_Yk, closest_point, qR_old, qT_old);
669 qT = mu_Yk - qR.rotate(mu_P);
674 d_k = compute_d_k(P_, closest_point, qR, qR_old, qT, qT_old);
678 image3d<value::rgb8> tmp_ =
duplicate(debug);
679 mln_piter(p_array<P>) p_dbg(P_);
681 tmp_(qR_old.
rotate(p_dbg.to_vec()) + qT_old) = literal::green;
682 std::ostringstream ss;
687 io::ppm::
save(mln::
slice(tmp_,0), ss.str());
689 std::cout << "e_" << k << "=" << e_k << std::endl;
690 std::cout << "d_" << k << "=" << d_k << std::endl;
703 if (k > 3 && (d_k > e_k || d_k > d_k_old || e_k > e_k_old))
719 return std::make_pair(qR, qT);
723 # endif // ! MLN_INCLUDE_ONLY
729 #endif // ! MLN_REGISTRATION_ICP_HH