00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #ifndef MLN_REGISTRATION_ICP_HH
00027 # define MLN_REGISTRATION_ICP_HH
00028
00034
00035 # include <cmath>
00036 # include <algorithm>
00037
00038 # include <mln/core/alias/vec3d.hh>
00039 # include <mln/math/jacobi.hh>
00040 # include <mln/fun/x2x/all.hh>
00041 # include <mln/fun/x2v/all.hh>
00042 # include <mln/convert/to.hh>
00043 # include <mln/accu/compute.hh>
00044 # include <mln/accu/center.hh>
00045 # include <mln/accu/rms.hh>
00046 # include <mln/trait/image_from_grid.hh>
00047 # include <mln/set/compute.hh>
00048
00049
00050 # include <mln/core/image/dmorph/slice_image.hh>
00051 # include <mln/core/image/imorph/tr_image.hh>
00052 # include <mln/core/image/dmorph/extension_fun.hh>
00053
00054 # include <mln/core/alias/neighb3d.hh>
00055
00056 # include <mln/transform/distance_and_closest_point_geodesic.hh>
00057 # include <mln/canvas/distance_geodesic.hh>
00058 # include <mln/pw/all.hh>
00059
00060 # include <mln/io/ppm/save.hh>
00061 # include <mln/io/pbm/save.hh>
00062
00063 # include <mln/labeling/colorize.hh>
00064 # include <mln/debug/histo.hh>
00065
00066 # include <mln/accu/histo.hh>
00067 # include <mln/accu/math/sum.hh>
00068
00069 # include <mln/value/int_u16.hh>
00070
00071 # include <mln/literal/black.hh>
00072 # include <mln/literal/white.hh>
00073 # include <mln/literal/colors.hh>
00074
00075 # include <mln/util/timer.hh>
00076
00077 # include <mln/io/cloud/save.hh>
00078
00079
00080 namespace mln
00081 {
00082
00083 namespace registration
00084 {
00085
00086
00087
00088 using namespace fun::x2x;
00089
00113 template <typename P, typename F>
00114 std::pair<algebra::quat,mln_vec(P)>
00115 icp(const p_array<P>& P_,
00116 const p_array<P>& X,
00117 const F& closest_point,
00118 const algebra::quat& initial_rot,
00119 const mln_vec(P)& initial_translation);
00120
00121
00132 template <typename P, typename F>
00133 composed< translation<P::dim,float>,rotation<P::dim,float> >
00134 icp(const p_array<P>& P_,
00135 const p_array<P>& X,
00136 const F& closest_point);
00137
00138
00139 # ifndef MLN_INCLUDE_ONLY
00140
00141
00143 template <typename P>
00144 class closest_point_with_map
00145 {
00146 typedef mln_image_from_grid(mln_grid(P), P) I;
00147 typedef mln_ch_value(I, unsigned) cp_ima_t;
00148 typedef mln_ch_value(I,value::int_u16) dmap_t;
00149
00150 public:
00151
00152 closest_point_with_map(const p_array<P>& X)
00153 {
00154 box3d box = geom::bbox(X);
00155 box.enlarge(0, box.nslis());
00156 box.enlarge(1, box.nrows());
00157 box.enlarge(2, box.ncols());
00158
00159 mln_postcondition(box.is_valid());
00160
00161 std::cout << "Map image defined on " << box << std::endl;
00162
00163 X_ = X;
00164 init(X, box);
00165 }
00166
00167 closest_point_with_map(const p_array<P>& X, const box<P>& box)
00168 {
00169 X_ = X;
00170 init(X, box);
00171 }
00172
00173 void init(const p_array<P>& X, const box<P>& box)
00174 {
00175 typedef mln_ch_value(I, bool) model_t;
00176 model_t model(box);
00177 data::fill(model, false);
00178 data::fill((model | X).rw(), true);
00179
00180
00181 typedef util::couple<mln_ch_value(model_t, value::int_u16),
00182 mln_ch_value(model_t, unsigned)> couple_t;
00183 couple_t cpl = transform::distance_and_closest_point_geodesic(X, box,
00184 c6(),
00185 mln_max(value::int_u16));
00186
00187 dmap_X_ = cpl.first();
00188 cp_ima_ = cpl.second();
00189
00190 mln_postcondition(cp_ima_.is_valid());
00191 mln_postcondition(cp_ima_.domain().is_valid());
00192 std::cout << "pmin = " << cp_ima_.domain().pmin() << std::endl;;
00193 std::cout << "pmax = " << cp_ima_.domain().pmax() << std::endl;;
00194
00195 #ifndef NDEBUG
00196 mln_ch_value(I, bool) debug2(box);
00197 data::fill(debug2, false);
00198 mln_ch_value(I, value::rgb8) debug(box);
00199 mln_piter(p_array<P>) p(X);
00200 for_all(p)
00201 {
00202 debug(p) = labeling::internal::random_color(value::rgb8());
00203 debug2(p) = true;
00204 }
00205 io::pbm::save(slice(debug2,0), "debug2-a.ppm");
00206
00207 mln_piter(I) pi(cp_ima_.domain());
00208 for_all(pi)
00209 {
00210 debug(pi) = debug(X[cp_ima_(pi)]);
00211 debug2(pi) = debug2(X[cp_ima_(pi)]);
00212 }
00213
00214 io::pbm::save(slice(debug2,0), "debug2-b.ppm");
00215 io::ppm::save(slice(debug,0), "debug.ppm");
00216 std::cout << "map saved" << std::endl;
00217 #endif
00218 }
00219
00220 mln_site(I) operator()(const mln_site(I)& p) const
00221 {
00222 return X_[cp_ima_(p)];
00223 }
00224
00225
00226
00227 dmap_t dmap_X_;
00228
00229 private:
00230 p_array<P> X_;
00231
00232 cp_ima_t cp_ima_;
00233
00234 };
00235
00236
00238 template <typename P>
00239 class closest_point_basic
00240 {
00241 typedef mln_image_from_grid(mln_grid(P), P) I;
00242 typedef p_array<P> X_t;
00243
00244 public:
00245
00246 closest_point_basic(const p_array<P>& X)
00247 : X_(X)
00248 {
00249 }
00250
00251 mln_site(I) operator()(const vec3d_f& v) const
00252 {
00253 vec3d_f best_x = X_[0];
00254
00255 float best_d = norm::l2_distance(v, best_x);
00256 mln_piter(X_t) X_i(X_);
00257 for_all(X_i)
00258 {
00259 vec3d_f X_i_vec = X_i;
00260 float d = norm::l2_distance(v, X_i_vec);
00261 if (d < best_d)
00262 {
00263 best_d = d;
00264 best_x = X_i_vec;
00265 }
00266 }
00267 return best_x;
00268 }
00269
00270 private:
00271 const p_array<P>& X_;
00272 };
00273
00274
00275 template <typename P>
00276 void
00277 draw_last_run(const box3d& box, const p_array<P>& kept,
00278 const p_array<P>& removed, const p_array<P>& X,
00279 const algebra::quat& qR, const vec3d_f qT)
00280 {
00281 typedef image3d<value::rgb8> result_t;
00282 result_t result(box);
00283 typedef extension_fun<result_t,pw::cst_<mln_value(result_t)> > ext_result_t;
00284 ext_result_t ext_result(result, pw::cst(value::rgb8(0,0,0)));
00285
00286 data::fill(ext_result, literal::black);
00287 data::fill((ext_result | X).rw(), literal::white);
00288
00289 mln_piter(p_array<P>) p(kept);
00290 for_all(p)
00291 ext_result(qR.rotate(p.to_vec()) + qT) = literal::green;
00292
00293 mln_piter(p_array<P>) p2(removed);
00294 for_all(p2)
00295 ext_result(qR.rotate(p2.to_vec()) + qT) = literal::red;
00296
00297 io::ppm::save(slice(ext_result,0), "registered-2.ppm");
00298 }
00299
00300
00301
00302 template <typename P, typename F>
00303 float compute_standard_deviation(const p_array<P>& P_,
00304 const std::pair<algebra::quat,mln_vec(P)>& pair,
00305 const F& closest_point)
00306 {
00307 accu::rms<vec3d_f,float> e_k_accu;
00308
00309
00310 float sd;
00311 mln_piter(p_array<P>) p(P_);
00312 for_all(p)
00313 {
00314 vec3d_f Pk_i = pair.first.rotate(p) + pair.second;
00315 vec3d_f Yk_i = closest_point(Pk_i).to_vec();
00316
00317 e_k_accu.take(Yk_i - Pk_i);
00318 }
00319
00320 float d = e_k_accu.to_result();
00321 sd = std::sqrt(e_k_accu.hook_value_() / P_.nsites() - d * d);
00322 return sd;
00323 }
00324
00325
00326 template <typename P, typename F>
00327 void
00328 remove_too_far_sites_debug(image3d<value::rgb8>& out, const p_array<P>& P_,
00329 const F& closest_point,
00330 const std::pair<algebra::quat,mln_vec(P)>& pair,
00331 const p_array<P>& X,
00332 unsigned r, int d_min, int d_max, unsigned prefix)
00333 {
00334 unsigned removed = 0;
00335 accu::histo<value::int_u8> h;
00336 mln_piter(p_array<P>) p(P_);
00337 data::fill(out, literal::black);
00338 data::fill((out | X).rw(), literal::white);
00339
00340 for_all(p)
00341 {
00342 vec3d_f Pk_i = pair.first.rotate(p) + pair.second;
00343 vec3d_f Yk_i = closest_point(Pk_i);
00344
00345 int d_i = closest_point.dmap_X_(Pk_i);
00346 if (d_i >= d_min && d_i <= d_max)
00347 out(Pk_i) = literal::green;
00348 else
00349 {
00350 ++removed;
00351 out(Pk_i) = literal::red;
00352 }
00353 }
00354
00355 #ifndef NDEBUG
00356 std::ostringstream ss1;
00357 ss1 << "histo_" << prefix << r << ".dat";
00358 std::cout << h << std::endl;
00359
00360 std::ostringstream ss2;
00361 ss2 << "out_" << prefix << r << ".ppm";
00362 io::ppm::save(mln::slice(out,0), ss2.str());
00363 #endif
00364 std::cout << "Points removed with the whole set and current d_min/d_max: " << removed << std::endl;
00365
00366 }
00367
00368
00369 template <typename P, typename F>
00370 void
00371 compute_distance_criteria(const p_array<P>& P_,
00372 const F& closest_point,
00373 const std::pair<algebra::quat,mln_vec(P)>& pair,
00374 unsigned r, int& d_min, int& d_max)
00375 {
00376 mln_piter(p_array<P>) p(P_);
00377 accu::histo<value::int_u8> h;
00378
00379 float sd;
00380 {
00381 accu::math::sum<float> s, s2;
00382 for_all(p)
00383 {
00384 vec3d_f Pk_i = pair.first.rotate(p.to_vec()) + pair.second;
00385 unsigned d_i = closest_point.dmap_X_(Pk_i);
00386 h.take(d_i);
00387 s.take(d_i);
00388 s2.take(d_i * d_i);
00389 }
00390 float mean = s / P_.nsites();
00391 sd = std::sqrt(s2 / P_.nsites() - mean * mean);
00392 d_min = int(mean - sd);
00393 d_max = int(mean + sd);
00394 }
00395
00396 std::cout << "Standard deviation = " << sd << std::endl;
00397 std::ostringstream ss1;
00398 ss1 << "histo_" << r << ".dat";
00399 std::cout << h << std::endl;
00400 std::cout << "d thresholds = " << d_min << ' ' << d_max << std::endl;
00401 }
00402
00403 template <typename P, typename F>
00404 p_array<P>
00405 remove_too_far_sites(image3d<value::rgb8>& out, const p_array<P>& P_,
00406 const F& closest_point,
00407 const std::pair<algebra::quat,mln_vec(P)>& pair,
00408 const p_array<P>& X, p_array<P>& removed_set,
00409 unsigned r, int d_min, int d_max,
00410 const std::string& method)
00411 {
00412 p_array<P> tmp;
00413 unsigned removed = 0;
00414
00415 # ifndef NDEBUG
00416 data::fill(out, literal::black);
00417 data::fill((out | X).rw(), literal::white);
00418 # endif // ! NDEBUG
00419
00420 mln_piter(p_array<P>) p(P_);
00421 for_all(p)
00422 {
00423 vec3d_f Pk_i = pair.first.rotate(p.to_vec()) + pair.second;
00424 vec3d_f Yk_i = closest_point(Pk_i);
00425
00426 int d_i = closest_point.dmap_X_(Pk_i);
00427 if (d_i >= d_min && d_i <= d_max)
00428 {
00429 tmp.append(p);
00430 out(Pk_i) = literal::green;
00431 }
00432 else
00433 {
00434 ++removed;
00435 removed_set.append(p);
00436 out(Pk_i) = literal::red;
00437 }
00438 }
00439
00440 {
00441 std::ostringstream ss2;
00442 ss2 << method << "_" << r << "_removed_sites" << ".cloud";
00443 io::cloud::save(removed_set, ss2.str());
00444 }
00445 {
00446 std::ostringstream ss2;
00447 ss2 << method << "_" << r << "_kept_sites" << ".cloud";
00448 io::cloud::save(tmp, ss2.str());
00449 }
00450
00451 # ifndef NDEBUG
00452 std::ostringstream ss2;
00453 ss2 << method << "_" << r << "_removed_sites" << ".ppm";
00454 io::ppm::save(mln::slice(out,0), ss2.str());
00455
00456 std::cout << "Points removed: " << removed << std::endl;
00457 # endif // ! NDEBUG
00458
00459
00460 (void) X;
00461 (void) r;
00462 (void) method;
00463
00464 return tmp;
00465 }
00466
00467 template <typename P>
00468 void
00469 display_sites_used_in_icp(image3d<value::rgb8>& out, const p_array<P>& P_sub,
00470 const p_array<P>& P_, const p_array<P>& X,
00471 unsigned r, const std::string& prefix,
00472 const std::pair<algebra::quat,mln_vec(P)>& pair,
00473 const std::string& period, const value::rgb8& c)
00474 {
00475 data::fill(out, literal::black);
00476 data::fill((out | X).rw(), literal::white);
00477
00478 mln_piter(p_array<P>) p1(P_);
00479 for_all(p1)
00480 {
00481 vec3d_f Pk_i = pair.first.rotate(p1.to_vec()) + pair.second;
00482 out(Pk_i) = literal::red;
00483 }
00484
00485 mln_piter(p_array<P>) p2(P_sub);
00486 for_all(p2)
00487 {
00488 vec3d_f Pk_i = pair.first.rotate(p2.to_vec()) + pair.second;
00489 out(Pk_i) = c;
00490 }
00491
00492 std::ostringstream ss;
00493 ss << prefix << "_" << r << "_" << period << ".ppm";
00494
00495 io::ppm::save(slice(out,0), ss.str());
00496 }
00497
00498
00499 template <typename P, typename F>
00500 inline
00501 float
00502 compute_d_k(const p_array<P>& P_,
00503 const F& closest_point,
00504 const algebra::quat& qR,
00505 const algebra::quat& qR_old,
00506 const vec3d_f& qT,
00507 const vec3d_f& qT_old)
00508 {
00509 accu::rms<vec3d_f, float> accu;
00510 mln_piter(p_array<P>) p(P_);
00511 for_all(p)
00512 {
00513
00514 vec3d_f P_i = p;
00515 vec3d_f Pk_i = qR_old.rotate(P_i) + qT_old;
00516 vec3d_f Pk_1_i = qR.rotate(P_i) + qT;
00517 accu.take(closest_point(Pk_i).to_vec() - Pk_1_i);
00518 }
00519 return accu.to_result();
00520 }
00521
00522
00524 template <typename P, typename F>
00525 algebra::quat
00526 get_rot(const p_array<P>& P_,
00527 const vec3d_f& mu_P,
00528 const vec3d_f& mu_Yk,
00529 const F& closest_point,
00530 const algebra::quat& qR,
00531 const vec3d_f& qT)
00532 {
00534 algebra::mat<3u,3u,float> Spx;
00535 mln_piter(p_array<P>) p(P_);
00536
00537
00538 for_all(p)
00539 {
00540 vec3d_f P_i = p;
00541 vec3d_f Pk_i = qR.rotate(P_i) + qT;
00542 vec3d_f Yk_i = closest_point(Pk_i);
00543 Spx += (P_i - mu_P) * (Yk_i - mu_Yk).t();
00544 }
00545 Spx /= P_.nsites();
00546
00547 vec3d_f A;
00548 A[0] = Spx(1,2) - Spx(2,1);
00549 A[1] = Spx(2,0) - Spx(0,2);
00550 A[2] = Spx(0,1) - Spx(1,0);
00551
00552 algebra::mat<4u,4u,float> Qk;
00553 float t = tr(Spx);
00554
00555 Qk(0,0) = t;
00556 for (int i = 1; i < 4; ++i)
00557 {
00558 Qk(i,0) = A[i - 1];
00559 Qk(0,i) = A[i - 1];
00560 for (int j = 1; j < 4; ++j)
00561 if (i == j)
00562 Qk(i,j) = 2 * Spx(i - 1,i - 1) - t;
00563 }
00564
00565 Qk(1,2) = Spx(0,1) + Spx(1,0);
00566 Qk(2,1) = Spx(0,1) + Spx(1,0);
00567
00568 Qk(1,3) = Spx(0,2) + Spx(2,0);
00569 Qk(3,1) = Spx(0,2) + Spx(2,0);
00570
00571 Qk(2,3) = Spx(1,2) + Spx(2,1);
00572 Qk(3,2) = Spx(1,2) + Spx(2,1);
00573
00574 return math::jacobi(Qk);
00575 }
00576
00577
00578
00579 template <typename P, typename F>
00580 inline
00581 vec3d_f
00582 get_mu_yk(const p_array<P>& P_,
00583 const F& closest_point,
00584 const algebra::quat& qR,
00585 const vec3d_f& qT,
00586 float& e_k)
00587 {
00588 accu::rms<vec3d_f,float> e_k_accu;
00589 accu::center<P,vec3d_f> mu_yk;
00590
00591 mln_piter(p_array<P>) p(P_);
00592 for_all(p)
00593 {
00594
00595 vec3d_f Pk_i = qR.rotate(p.to_vec()) + qT;
00596 vec3d_f Yk_i = closest_point(Pk_i);
00597 mu_yk.take(Yk_i);
00598 e_k_accu.take(Yk_i - Pk_i);
00599 }
00600
00601 e_k = e_k_accu.to_result();
00602 return mu_yk.to_result();
00603 }
00604
00605
00606
00608 template <typename P, typename F>
00609 inline
00610 std::pair<algebra::quat,mln_vec(P)>
00611 icp(const p_array<P>& P_,
00612 const p_array<P>& X,
00613 const F& closest_point,
00614 const algebra::quat& initial_rot,
00615 const mln_vec(P)& initial_translation)
00616 {
00617 trace::entering("registration::icp");
00618
00619 (void) X;
00620 mln_precondition(P::dim == 3);
00621 mln_precondition(!P_.is_empty());
00622 mln_precondition(!X.is_empty());
00623 mln_precondition(!initial_rot.is_null());
00624
00625 typedef p_array<P> cloud_t;
00626
00627 vec3d_f mu_P = set::compute(accu::center<P,vec3d_f>(), P_);
00628
00629 vec3d_f qT_old, qT = initial_translation;
00630 algebra::quat qR_old, qR = initial_rot;
00631 float e_k, e_k_old = mln_max(float);
00632 float d_k, d_k_old = mln_max(float);
00633 unsigned k = 0;
00634
00635 # ifndef NDEBUG
00636 box3d box = geom::bbox(X);
00637
00638 box.enlarge(1, box.nrows() / 2);
00639 box.enlarge(2, box.ncols() / 2);
00640 image3d<value::rgb8> debug(box);
00641 data::fill(debug, literal::black);
00642 data::fill((debug | X).rw(), literal::white);
00643 # endif
00644
00645 do
00646 {
00647 qT_old = qT;
00648 qR_old = qR;
00649
00652
00653
00654 vec3d_f mu_Yk = get_mu_yk(P_, closest_point, qR_old, qT_old, e_k);
00655
00656
00657 qR = get_rot(P_, mu_P, mu_Yk, closest_point, qR_old, qT_old);
00658 vec3d_f tmp = qR.v();
00659
00660
00661 qT = mu_Yk - qR.rotate(mu_P);
00664
00665
00666 d_k = compute_d_k(P_, closest_point, qR, qR_old, qT, qT_old);
00667
00668
00669 #ifndef NDEBUG
00670 image3d<value::rgb8> tmp_ = duplicate(debug);
00671 mln_piter(p_array<P>) p_dbg(P_);
00672 for_all(p_dbg)
00673 tmp_(qR_old.rotate(p_dbg.to_vec()) + qT_old) = literal::green;
00674 std::ostringstream ss;
00675 ss << "tmp_0";
00676 if (k < 10)
00677 ss << "0";
00678 ss << k << ".ppm";
00679 io::ppm::save(mln::slice(tmp_,0), ss.str());
00680 #endif
00681
00682 std::cout << "e_" << k << "=" << e_k << std::endl;
00683 std::cout << "d_" << k << "=" << d_k << std::endl;
00684
00685
00686
00687
00688
00689
00690
00691
00692
00693
00694
00695 if (k > 3 && (d_k > e_k || d_k > d_k_old || e_k > e_k_old))
00696 {
00697 qR = qR_old;
00698 qT = qT_old;
00699 break;
00700 }
00701
00702
00703 d_k_old = d_k;
00704 e_k_old = e_k;
00705
00706 ++k;
00707
00708 } while ((k < 3)
00709 || norm::l2((qT - qT_old)) + norm::l2((qR - qR_old).to_vec()) > 1e-3);
00710
00711 trace::exiting("registration::icp");
00712 return std::make_pair(qR, qT);
00713 }
00714
00715
00716 # endif // ! MLN_INCLUDE_ONLY
00717
00718 }
00719
00720 }
00721
00722 #endif // ! MLN_REGISTRATION_ICP_HH