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;
114 template <
typename P,
typename F>
115 std::pair<algebra::quat,mln_vec(P)>
116 icp(
const p_array<P>& P_,
118 const F& closest_point,
119 const algebra::quat& initial_rot,
120 const mln_vec(P)& initial_translation);
133 template <
typename P,
typename F>
134 composed< translation<P::dim,float>,rotation<P::dim,float> >
135 icp(
const p_array<P>& P_,
137 const F& closest_point);
140 # ifndef MLN_INCLUDE_ONLY
144 template <
typename P>
147 typedef mln_image_from_grid(mln_grid(P), P) I;
148 typedef mln_ch_value(I,
unsigned) cp_ima_t;
162 std::cout <<
"Map image defined on " << box << std::endl;
176 typedef mln_ch_value(I,
bool) model_t;
183 mln_ch_value(model_t,
unsigned)> couple_t;
188 dmap_X_ = cpl.first();
189 cp_ima_ = cpl.second();
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;;
197 mln_ch_value(I,
bool) debug2(box);
203 debug(p) = labeling::internal::random_color(
value::rgb8());
208 mln_piter(I) pi(cp_ima_.domain());
211 debug(pi) = debug(X[cp_ima_(pi)]);
212 debug2(pi) = debug2(X[cp_ima_(pi)]);
217 std::cout <<
"map saved" << std::endl;
221 mln_site(I) operator()(
const mln_site(I)& p)
const
223 return X_[cp_ima_(p)];
239 template <
typename P>
242 typedef mln_image_from_grid(mln_grid(P), P) I;
252 mln_site(I) operator()(
const vec3d_f& v)
const
257 mln_piter(
X_t) X_i(X_);
276 template <
typename P>
280 const algebra::quat& qR,
const vec3d_f qT)
283 result_t result(box);
285 ext_result_t ext_result(result, pw::cst(
value::rgb8(0,0,0)));
292 ext_result(qR.rotate(p.to_vec()) + qT) = literal::green;
294 mln_piter(
p_array<P>) p2(removed);
296 ext_result(qR.rotate(p2.to_vec()) + qT) = literal::red;
298 io::ppm::save(slice(ext_result,0), "registered-2.ppm");
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)
315 vec3d_f Pk_i = pair.first.rotate(p) + pair.second;
316 vec3d_f Yk_i = closest_point(Pk_i).to_vec();
318 e_k_accu.take(Yk_i - Pk_i);
322 sd = std::sqrt(e_k_accu.hook_value_() / P_.
nsites() - d * d);
327 template <
typename P,
typename F>
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,
333 unsigned r,
int d_min,
int d_max,
unsigned prefix)
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);
343 vec3d_f Pk_i = pair.first.rotate(p) + pair.second;
344 vec3d_f Yk_i = closest_point(Pk_i);
346 int d_i = closest_point.dmap_X_(Pk_i);
347 if (d_i >= d_min && d_i <= d_max)
357 std::ostringstream ss1;
358 ss1 <<
"histo_" << prefix << r <<
".dat";
359 std::cout << h << std::endl;
361 std::ostringstream ss2;
362 ss2 <<
"out_" << prefix << r <<
".ppm";
365 std::cout <<
"Points removed with the whole set and current d_min/d_max: " << removed << std::endl;
370 template <
typename P,
typename F>
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)
377 mln_piter(p_array<P>) p(P_);
378 accu::histo<value::int_u8> h;
382 accu::math::sum<float> s, s2;
385 vec3d_f Pk_i = pair.first.rotate(p.to_vec()) + pair.second;
386 unsigned d_i = closest_point.dmap_X_(Pk_i);
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);
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;
404 template <
typename P,
typename F>
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)
414 unsigned removed = 0;
421 mln_piter(p_array<P>) p(P_);
424 vec3d_f Pk_i = pair.first.rotate(p.to_vec()) + pair.second;
425 vec3d_f Yk_i = closest_point(Pk_i);
427 int d_i = closest_point.dmap_X_(Pk_i);
428 if (d_i >= d_min && d_i <= d_max)
436 removed_set.append(p);
442 std::ostringstream ss2;
443 ss2 << method <<
"_" << r <<
"_removed_sites" <<
".cloud";
447 std::ostringstream ss2;
448 ss2 << method <<
"_" << r <<
"_kept_sites" <<
".cloud";
453 std::ostringstream ss2;
454 ss2 << method <<
"_" << r <<
"_removed_sites" <<
".ppm";
457 std::cout <<
"Points removed: " << removed << std::endl;
468 template <
typename P>
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,
479 mln_piter(p_array<P>) p1(P_);
482 vec3d_f Pk_i = pair.first.rotate(p1.to_vec()) + pair.second;
486 mln_piter(p_array<P>) p2(P_sub);
489 vec3d_f Pk_i = pair.first.rotate(p2.to_vec()) + pair.second;
493 std::ostringstream ss;
494 ss << prefix <<
"_" << r <<
"_" << period <<
".ppm";
500 template <
typename P,
typename F>
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,
510 accu::rms<vec3d_f, float> accu;
511 mln_piter(p_array<P>) p(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);
520 return accu.to_result();
525 template <
typename P,
typename F>
530 const F& closest_point,
531 const algebra::quat& qR,
535 algebra::mat<3u,3u,float> Spx;
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();
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);
553 algebra::mat<4u,4u,float> Qk;
557 for (
int i = 1; i < 4; ++i)
561 for (
int j = 1; j < 4; ++j)
563 Qk(i,j) = 2 * Spx(i - 1,i - 1) - t;
566 Qk(1,2) = Spx(0,1) + Spx(1,0);
567 Qk(2,1) = Spx(0,1) + Spx(1,0);
569 Qk(1,3) = Spx(0,2) + Spx(2,0);
570 Qk(3,1) = Spx(0,2) + Spx(2,0);
572 Qk(2,3) = Spx(1,2) + Spx(2,1);
573 Qk(3,2) = Spx(1,2) + Spx(2,1);
575 return math::jacobi(Qk);
580 template <
typename P,
typename F>
584 const F& closest_point,
585 const algebra::quat& qR,
596 vec3d_f Pk_i = qR.rotate(p.to_vec()) + qT;
597 vec3d_f Yk_i = closest_point(Pk_i);
599 e_k_accu.take(Yk_i - Pk_i);
609 template <
typename P,
typename F>
611 std::pair<algebra::quat,mln_vec(P)>
614 const F& closest_point,
615 const algebra::quat& initial_rot,
616 const mln_vec(P)& initial_translation)
618 trace::entering(
"registration::icp");
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());
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);
639 box.
enlarge(1, box.nrows() / 2);
640 box.
enlarge(2, box.ncols() / 2);
655 vec3d_f mu_Yk = get_mu_yk(P_, closest_point, qR_old, qT_old, e_k);
658 qR = get_rot(P_, mu_P, mu_Yk, closest_point, qR_old, qT_old);
662 qT = mu_Yk - qR.rotate(mu_P);
667 d_k = compute_d_k(P_, closest_point, qR, qR_old, qT, qT_old);
675 std::ostringstream ss;
683 std::cout <<
"e_" << k <<
"=" << e_k << std::endl;
684 std::cout <<
"d_" << k <<
"=" << d_k << std::endl;
696 if (k > 3 && (d_k > e_k || d_k > d_k_old || e_k > e_k_old))
710 || norm::l2((qT - qT_old)) + norm::l2((qR - qR_old).to_vec()) > 1e-3);
712 trace::exiting(
"registration::icp");
713 return std::make_pair(qR, qT);
717 # endif // ! MLN_INCLUDE_ONLY
723 #endif // ! MLN_REGISTRATION_ICP_HH