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_REGISTRATION_HH
00027 # define MLN_REGISTRATION_REGISTRATION_HH
00028
00033
00034 # include <mln/core/image/image3d.hh>
00035 # include <mln/core/site_set/box.hh>
00036 # include <mln/registration/icp.hh>
00037 # include <mln/fun/x2x/all.hh>
00038 # include <mln/fun/x2p/closest_point.hh>
00039 # include <mln/convert/to_p_array.hh>
00040
00041
00042 # include <mln/util/timer.hh>
00043
00044 namespace mln
00045 {
00046
00047 namespace registration
00048 {
00049
00050 using namespace mln::fun::x2x;
00051
00052
00054 template <typename P>
00055 inline
00056 composed< translation<P::dim,float>,rotation<P::dim,float> >
00057 registration1(const box<P>& domain,
00058 const p_array<P>& P_,
00059 const p_array<P>& X);
00060
00068 template <typename P>
00069 inline
00070 composed< translation<P::dim,float>,rotation<P::dim,float> >
00071 registration2(const box<P>& domain,
00072 const p_array<P>& P_,
00073 const p_array<P>& X);
00074
00081 template <typename P>
00082 inline
00083 composed< translation<P::dim,float>,rotation<P::dim,float> >
00084 registration3(const box<P>& domain,
00085 const p_array<P>& P_,
00086 const p_array<P>& X);
00087
00088
00089
00090 # ifndef MLN_INCLUDE_ONLY
00091
00092
00093 namespace internal
00094 {
00095
00096 template <typename P>
00097 inline
00098 void
00099 registration_tests(const p_array<P>& P_, const p_array<P>& X)
00100 {
00101 mln_assertion(P_.is_valid());
00102 mln_assertion(X.is_valid());
00103 mln_assertion(!X.is_empty());
00104 mln_assertion(!P_.is_empty());
00105
00106
00107 mln_precondition(P::dim == 3);
00108 (void) P_;
00109 (void) X;
00110 }
00111
00112 }
00113
00114
00115 namespace impl
00116 {
00117
00118 template <typename P>
00119 inline
00120 composed< translation<P::dim,float>,rotation<P::dim,float> >
00121 registration1(const box<P>& domain,
00122 const p_array<P>& P_,
00123 const p_array<P>& X)
00124 {
00125 trace::entering("mln::registration::registration1");
00126
00127 # ifndef NDEBUG
00128 util::timer t;
00129 t.start();
00130 # endif // ! NDEBUG
00131
00132 registration::closest_point_with_map<P> closest_point(X, domain);
00133
00134 std::pair<algebra::quat,mln_vec(P)> pair = icp(P_, X, closest_point,
00135 algebra::quat(1,0,0,0),
00136 literal::zero);
00137 # ifndef NDEBUG
00138 std::cout << "icp = " << t << std::endl;
00139 # endif // ! NDEBUG
00140
00141 typedef rotation<3u,float> rot_t;
00142 rot_t tqR(pair.first);
00143 typedef translation<3u,float> trans_t;
00144 trans_t tqT(pair.second);
00145 composed<trans_t, rot_t> result(tqT, tqR);
00146
00147 trace::exiting("mln::registration::registration1");
00148
00149 return result;
00150 }
00151
00152
00153 template <typename P>
00154 inline
00155 composed< translation<P::dim,float>,rotation<P::dim,float> >
00156 registration2(const box<P>& domain,
00157 const p_array<P>& P_,
00158 const p_array<P>& X)
00159 {
00160 trace::entering("mln::registration::registration2");
00161
00162
00163 std::string method = "registration2";
00164
00165 registration::closest_point_with_map<P> closest_point(X, domain);
00166
00167 # ifndef NDEBUG
00168 util::timer t;
00169 t.start();
00170 # endif // ! NDEBUG
00171
00172
00173 p_array<P> P_bak = P_;
00174
00175 unsigned r = 0;
00176 std::pair<algebra::quat,mln_vec(P)> pair;
00177 pair.first = algebra::quat(1,0,0,0);
00178 pair.second = literal::zero;
00179
00180
00181 image3d<value::rgb8> out(domain);
00182
00183 p_array<P> removed_set;
00184
00185 do
00186 {
00187
00188 # ifndef NDEBUG
00189 std::cout << std::endl << std::endl << "==== New run - " << r << std::endl;
00190 # endif // ! NDEBUG
00191
00192 pair = icp(P_bak, X, closest_point,
00193 pair.first,
00194 pair.second);
00195
00196 # ifndef NDEBUG
00197 display_sites_used_in_icp(out, P_bak, P_, X, r, method, pair,
00198 "final", literal::blue);
00199 # endif // ! NDEBUG
00200
00201 int d_min, d_max;
00202 compute_distance_criteria(P_bak, closest_point, pair, r, d_min, d_max);
00203
00204 P_bak = remove_too_far_sites(out, P_bak,
00205 closest_point, pair, X, removed_set,
00206 r, d_min, d_max, method);
00207
00208 # ifndef NDEBUG
00209 display_sites_used_in_icp(out, P_bak, P_, X, r, method, pair,
00210 "schanges", literal::green);
00211 std::cout << "==== End of run" << std::endl;
00212 # endif
00213
00214 ++r;
00215
00216 } while (r < 10);
00217
00218 # ifndef NDEBUG
00219 std::cout << "icp = " << t << std::endl;
00220 draw_last_run(domain, P_bak, removed_set, X, pair.first, pair.second);
00221 # endif
00222
00223 typedef rotation<3u,float> rot_t;
00224 rot_t tqR(pair.first);
00225 typedef translation<3u,float> trans_t;
00226 trans_t tqT(pair.second);
00227 composed<trans_t,rot_t> result(tqT, tqR);
00228
00229 trace::exiting("mln::registration::registration2");
00230
00231 return result;
00232 }
00233
00234
00235 template <typename P>
00236 inline
00237 composed< translation<P::dim,float>,rotation<P::dim,float> >
00238 registration3(const box<P>& domain,
00239 const p_array<P>& P_,
00240 const p_array<P>& X)
00241 {
00242 trace::entering("mln::registration::registration3");
00243
00244 registration::closest_point_with_map<P> closest_point(X, domain);
00245
00246
00247 std::string method = "registration3";
00248
00249 # ifndef NDEBUG
00250 util::timer t;
00251 t.start();
00252 # endif // ! NDEBUG
00253
00254
00255 p_array<P> P_bak = P_;
00256
00257 unsigned r = 0;
00258 std::pair<algebra::quat,mln_vec(P)> pair;
00259 pair.first = algebra::quat(1,0,0,0);
00260 pair.second = literal::zero;
00261
00262
00263 image3d<value::rgb8> out(domain);
00264
00265 p_array<P> removed_set;
00266
00267 do
00268 {
00269 # ifndef NDEBUG
00270 std::cout << std::endl << std::endl << "==== New run - "
00271 << r << std::endl;
00272 # endif // ! NDEBUG
00273
00274 pair = icp(P_bak, X, closest_point,
00275 pair.first,
00276 pair.second);
00277
00278 # ifndef NDEBUG
00279 display_sites_used_in_icp(out, P_bak, P_, X, r, method, pair,
00280 "final", literal::blue);
00281 # endif // ! NDEBUG
00282
00283 int d_min, d_max;
00284 compute_distance_criteria(P_bak, closest_point, pair, r, d_min, d_max);
00285
00286 P_bak = remove_too_far_sites(out, P_,
00287 closest_point, pair, X, removed_set,
00288 r, d_min, d_max, method);
00289
00290 # ifndef NDEBUG
00291 display_sites_used_in_icp(out, P_bak, P_, X, r, method, pair,
00292 "schanges", literal::green);
00293 std::cout << "==== End of run" << std::endl;
00294 # endif // ! NDEBUG
00295
00296 ++r;
00297
00298 } while (r < 10);
00299
00300 # ifndef NDEBUG
00301 std::cout << "icp = " << t << std::endl;
00302 draw_last_run(domain, P_bak, removed_set, X, pair.first, pair.second);
00303 # endif // ! NDEBUG
00304
00305 typedef rotation<3u,float> rot_t;
00306 rot_t tqR(pair.first);
00307 typedef translation<3u,float> trans_t;
00308 trans_t tqT(pair.second);
00309 composed<trans_t,rot_t> result(tqT, tqR);
00310
00311 trace::exiting("mln::registration::registration3");
00312
00313 return result;
00314 }
00315
00316 }
00317
00318
00319
00320
00321
00322 template <typename P>
00323 inline
00324 composed< translation<P::dim,float>,rotation<P::dim,float> >
00325 registration1(const box<P>& domain,
00326 const p_array<P>& cloud,
00327 const p_array<P>& surface)
00328 {
00329 trace::entering("registration::registration1");
00330
00331 internal::registration_tests(cloud, surface);
00332
00333 composed< translation<P::dim,float>, rotation<P::dim,float> >
00334 qk = impl::registration1(domain, cloud, surface);
00335
00336 trace::exiting("registration::registration1");
00337
00338 return qk;
00339 }
00340
00341
00342 template <typename P>
00343 inline
00344 composed< translation<P::dim,float>,rotation<P::dim,float> >
00345 registration2(const box<P>& domain,
00346 const p_array<P>& cloud,
00347 const p_array<P>& surface)
00348 {
00349 trace::entering("registration::registration2");
00350
00351 internal::registration_tests(cloud, surface);
00352
00353 composed< translation<P::dim,float>, rotation<P::dim,float> >
00354 qk = impl::registration2(domain, cloud, surface);
00355
00356 trace::exiting("registration::registration2");
00357
00358 return qk;
00359 }
00360
00361
00362 template <typename P>
00363 inline
00364 composed< translation<P::dim,float>,rotation<P::dim,float> >
00365 registration3(const box<P>& domain,
00366 const p_array<P>& cloud,
00367 const p_array<P>& surface)
00368 {
00369 trace::entering("registration::registration3");
00370
00371 internal::registration_tests(cloud, surface);
00372
00373 composed< translation<P::dim,float>, rotation<P::dim,float> >
00374 qk = impl::registration3(domain, cloud, surface);
00375
00376 trace::exiting("registration::registration3");
00377
00378 return qk;
00379 }
00380
00381
00382 # endif // ! MLN_INCLUDE_ONLY
00383
00384
00385 }
00386
00387
00388 }
00389
00390
00391 #endif // ! MLN_REGISTRATION_REGISTRATION_HH