00001 #ifndef SAMPLING_MATCHER_H
00002 #define SAMPLING_MATCHER_H
00003
00004 #include "polygonal_curve.h"
00005 #include "rescutil.h"
00006
00016 namespace rescue
00017 {
00018
00039
00040
00041
00042 template<class fs_graph>
00043 class sampling_matcher
00044 {
00045
00046 public:
00047
00048 typedef fs_graph::transporter_edge_class transporter_edge;
00049 typedef fs_graph::motion motion;
00050 typedef fs_graph::arithmetic_t arithmetic;
00051 typedef arithmetic arithmetic_t;
00052 typedef fs_graph::index_t index_t;
00053 typedef fs_graph::point point;
00054 typedef fs_graph::discrete_reparametrization_pair discrete_reparametrization_pair;
00055
00056 typedef polygonal_curve<point> curve;
00057 typedef curve::vertex vertex;
00058 typedef curve::edge edge;
00059
00060 protected:
00061
00064 index_t m;
00065
00068 index_t n;
00069
00072 index_t* kappa;
00073
00076 index_t* lambda;
00077
00081 point* P;
00082
00086 point* Q;
00087
00090 arithmetic eps;
00091
00094 arithmetic delta;
00095
00099 bool matched;
00100
00104 bool match_found;
00105
00109 bool parametrizations_available;
00110
00113 fs_graph Gamma;
00114
00118 motion match_motion;
00119
00120
00124 discrete_reparametrization_pair match_kappa_lambda;
00125
00128 static inline arithmetic dist(const point& x, const point& y);
00129
00130 public:
00131
00136 bool get_reparametrizations(discrete_reparametrization_pair&);
00137
00138
00143 sampling_matcher(curve& P, curve& Q, const arithmetic& ee);
00144
00149 sampling_matcher(curve& cP, curve& cQ, const arithmetic& ee, const arithmetic& dd);
00150
00153 sampling_matcher();
00154
00157 ~sampling_matcher();
00158
00162 bool match();
00163
00167 motion get_match();
00168
00173 bool get_reparametrized_curves(curve& P_kappa, curve& Q_lambda);
00174
00177 void clear();
00178
00179 };
00180
00181
00182
00183
00184
00185
00186 template<class fs_graph>
00187 sampling_matcher<fs_graph>::sampling_matcher()
00188
00189 {
00190
00191 m = 0;
00192 n = 0;
00193
00194 kappa = NULL;
00195 lambda = NULL;
00196
00197 P = NULL;
00198 Q = NULL;
00199
00200 eps = 0.0;
00201 delta = 0.0;
00202
00203 parametrizations_available = false;
00204 matched = false;
00205 match_found = false;
00206 }
00207
00208
00209
00210 template<class fs_graph>
00211 sampling_matcher<fs_graph>::sampling_matcher(curve& cP, curve& cQ, const arithmetic& ee, const arithmetic& dd)
00212
00213 {
00214
00215 parametrizations_available = false;
00216 matched = false;
00217 match_found = false;
00218
00219 if (cP.length()<0 || cQ.length()<0)
00220 {
00221
00222 m = -1;
00223 n = -1;
00224
00225 kappa = NULL;
00226 lambda = NULL;
00227
00228 P = NULL;
00229 Q = NULL;
00230
00231 eps = 0.0;
00232 delta = 0.0;
00233
00234 return;
00235 }
00236
00237 eps = ee;
00238 delta = dd;
00239
00240 kappa = new index_t[m+n+2];
00241 lambda = new index_t[m+n+2];
00242
00243
00244
00245 curve::vertex_iterator v_it, v_end;
00246 v_it = cP.vertex_begin();
00247 v_end = cP.vertex_end();
00248
00249 std::list<point> P_cp;
00250 point P0,P1;
00251 P1 = (*v_it).pos;
00252 P_cp.push_back(P1);
00253 v_it++;
00254
00255 for (; v_it!=v_end; v_it++)
00256 {
00257
00258
00259 P0 = P1;
00260 P1 = (*v_it).pos;
00261
00262 arithmetic length = sqrt((P0.x-P1.x)*(P0.x-P1.x)+(P0.y-P1.y)*(P0.y-P1.y));
00263 arithmetic Dx = 2.0*delta*(P1.x-P0.x)/length;
00264 arithmetic Dy = 2.0*delta*(P1.y-P0.y)/length;
00265
00266 arithmetic covered = 2.0*delta;
00267
00268 while (covered<=length)
00269 {
00270 P0.x += Dx;
00271 P0.y += Dy;
00272 P_cp.push_back(P0);
00273 covered += 2.0*delta;
00274 }
00275
00276 P_cp.push_back(P1);
00277
00278 }
00279
00280 m = P_cp.size()-1;
00281 P = new point[P_cp.size()];
00282 std::list<point>::iterator p_it = P_cp.begin();
00283 index_t i=0;
00284
00285 for (; p_it!=P_cp.end(); p_it++)
00286 {
00287 P[i]=*p_it;
00288 i++;
00289 }
00290
00291 std::list<point> Q_cp;
00292 v_it = cQ.vertex_begin();
00293 v_end = cQ.vertex_end();
00294 point Q0,Q1;
00295 Q1 = (*v_it).pos;
00296 Q_cp.push_back(Q1);
00297 v_it++;
00298
00299 for (; v_it!=v_end; v_it++)
00300 {
00301
00302
00303 Q0 = Q1;
00304 Q1 = (*v_it).pos;
00305
00306 arithmetic length = sqrt((Q0.x-Q1.x)*(Q0.x-Q1.x)+(Q0.y-Q1.y)*(Q0.y-Q1.y));
00307 arithmetic Dx = 2.0*delta*(Q1.x-Q0.x)/length;
00308 arithmetic Dy = 2.0*delta*(Q1.y-Q0.y)/length;
00309
00310 arithmetic covered = 2.0*delta;
00311
00312 while (covered<=length)
00313 {
00314 Q0.x += Dx;
00315 Q0.y += Dy;
00316 Q_cp.push_back(Q0);
00317 covered += 2.0*delta;
00318 }
00319
00320 Q_cp.push_back(Q1);
00321
00322 }
00323
00324 n = Q_cp.size()-1;
00325 Q = new point[Q_cp.size()];
00326 std::list<point>::iterator q_it = Q_cp.begin();
00327 index_t j=0;
00328
00329 for (; q_it!=Q_cp.end(); q_it++)
00330 {
00331 Q[j]=*q_it;
00332 j++;
00333 }
00334
00335 }
00336
00337
00338
00339 template<class fs_graph>
00340 sampling_matcher<fs_graph>::sampling_matcher(curve& cP, curve& cQ, const arithmetic& ee)
00341
00342 {
00343
00344 parametrizations_available = false;
00345 matched = false;
00346 match_found = false;
00347
00348 if (cP.length()<0 || cQ.length()<0)
00349 {
00350
00351 m = -1;
00352 n = -1;
00353
00354 kappa = NULL;
00355 lambda = NULL;
00356
00357 P = NULL;
00358 Q = NULL;
00359
00360 eps = 0.0;
00361 delta = 0.0;
00362
00363 return;
00364 }
00365
00366 eps = ee;
00367 delta = 0.0;
00368
00369 kappa = new index_t[m+n+2];
00370 lambda = new index_t[m+n+2];
00371
00372 m = cP.length();
00373 P = new point[cP.length()+1];
00374 Curve::vertex_iterator p_it = cP.vertex_begin();
00375 index_t i=0;
00376
00377 for (; p_it!=cP.vertex_end(); p_it++)
00378 {
00379 P[i]=(*p_it).pos;
00380 i++;
00381 }
00382
00383 n = cQ.length();
00384 Q = new point[cQ.length()+1];
00385 Curve::vertex_iterator q_it = cQ.vertex_begin();
00386 i=0;
00387
00388 for (; q_it!=cQ.vertex_end(); q_it++)
00389 {
00390 Q[i]=(*q_it).pos;
00391 i++;
00392 }
00393
00394 }
00395
00396
00397 template<class fs_graph>
00398 bool sampling_matcher<fs_graph>::match()
00399
00400 {
00401
00402 if (m<0 || n<0)
00403 return false;
00404
00405 Gamma.build(P,Q,m,n,eps,delta);
00406
00407
00408 if (m==0 && n==0)
00409 {
00410 match_motion = Gamma.get_match();
00411 return true;
00412 }
00413
00414 fs_graph::border_iterator b_it = Gamma.border_begin();
00415 fs_graph::border_iterator b_end = Gamma.border_end();
00416
00417 int upd_cnt = 0;
00418 for (; b_it!=b_end; b_it++)
00419 {
00420 upd_cnt++;
00421
00422 if (Gamma.update(*b_it))
00423 {
00424 match_motion = Gamma.get_match();
00425 parametrizations_available = Gamma.get_reparametrizations(match_kappa_lambda);
00426 return true;
00427 }
00428
00429 }
00430
00431 Gamma.clear();
00432
00433 return false;
00434
00435 }
00436
00437
00438
00439 template<class fs_graph>
00440 sampling_matcher<fs_graph>::motion sampling_matcher<fs_graph>::get_match()
00441
00442 {
00443 return match_motion;
00444 }
00445
00446
00447 template<class fs_graph>
00448 void sampling_matcher<fs_graph>::clear()
00449
00450 {
00451 if (P!=NULL)
00452 delete[] P;
00453 if (Q!=NULL)
00454 delete[] Q;
00455 if (kappa!=NULL)
00456 delete[] kappa;
00457 if (lambda!=NULL)
00458 delete[] lambda;
00459
00460 P = NULL;
00461 Q = NULL;
00462 kappa = NULL;
00463 lambda = NULL;
00464
00465 m = 0;
00466 n = 0;
00467
00468 eps = 0.0;
00469 delta = 0.0;
00470
00471 Gamma.clear();
00472
00473 }
00474
00475
00476 template<class fs_graph>
00477 sampling_matcher<fs_graph>::~sampling_matcher()
00478
00479 {
00480 clear();
00481 }
00482
00483
00484
00485 template<class fs_graph>
00486 inline sampling_matcher<fs_graph>::arithmetic sampling_matcher<fs_graph>::dist(const point& P, const point& Q)
00487
00488 {
00489 return sqrt(sqr<arithmetic>(P.x-Q.x)+sqr<arithmetic>(P.y-Q.y));
00490 }
00491
00492
00493 template<class fs_graph>
00494 bool sampling_matcher<fs_graph>::get_reparametrizations(sampling_matcher<fs_graph>::discrete_reparametrization_pair& d)
00495
00496 {
00497 if (!reparametrizations_available)
00498 return false;
00499 kappa_lambda = match_kappa_lambda;
00500 return true;
00501 }
00502
00503
00504
00505 template<class fs_graph>
00506 bool sampling_matcher<fs_graph>::
00507 get_reparametrized_curves(curve& P_kappa, curve& Q_lambda)
00508
00509 {
00510 P_kappa.clear();
00511 Q_lambda.clear();
00512
00513 if (parametrizations_available==false)
00514 return false;
00515
00516 typename fs_graph::discrete_reparametrization_pair::iterator kl_pos = match_kappa_lambda.begin();
00517 typename fs_graph::discrete_reparametrization_pair::iterator kl_end = match_kappa_lambda.end();
00518
00519 for (; kl_pos!=kl_end; kl_pos++)
00520 {
00521 P_kappa.push_back(new polygon_vertex<point>(P[kl_pos->first]));
00522 point gQ = Q[kl_pos->second];
00523 gQ *= match_motion;
00524 Q_lambda.push_back(new polygon_vertex<point>(gQ));
00525 }
00526
00527 return true;
00528
00529 }
00530
00531
00532
00533
00534
00535
00536
00537 }
00538
00539 #endif