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