00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #include "models/shape/accumulators/ht_accum.h"
00025
00026 using namespace std;
00027
00028 namespace firevision {
00029 #if 0
00030 }
00031 #endif
00032
00033 RhtXNode* RhtXNode::reuse_head = NULL;
00034 RhtYNode* RhtYNode::reuse_head = NULL;
00035 RhtRNode* RhtRNode::reuse_head = NULL;
00036
00037 RhtXNode* RhtXNode::reuse_tail = NULL;
00038 RhtYNode* RhtYNode::reuse_tail = NULL;
00039 RhtRNode* RhtRNode::reuse_tail = NULL;
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062 RhtAccNode::RhtAccNode()
00063 {
00064 left = right = next = NULL;
00065 }
00066
00067
00068
00069 RhtAccNode::~RhtAccNode()
00070 {
00071 }
00072
00073
00074
00075
00076 void
00077 RhtAccNode::clear(int ignore)
00078 {
00079 left = right = NULL;
00080 }
00081
00082
00083
00084
00085
00086 RhtXNode::RhtXNode(int x)
00087 : RhtAccNode()
00088 {
00089 this->x=x;
00090 y_root = NULL;
00091 }
00092
00093
00094
00095
00096
00097
00098
00099
00100 int
00101 RhtXNode::insert(int x0, int y0, int r0)
00102 {
00103 if (x == x0)
00104 {
00105 if (!y_root)
00106 y_root = RhtYNode::generate(y0);
00107 return y_root->insert(y0, r0);
00108 }
00109 else if (x0 < x)
00110 {
00111 if (!left)
00112 left = generate(x0);
00113 return ((RhtXNode*)left)->insert(x0, y0, r0);
00114 }
00115 else
00116 {
00117 if (!right)
00118 right = generate(x0);
00119 return ((RhtXNode*)right)->insert(x0, y0, r0);
00120 }
00121 }
00122
00123
00124
00125
00126
00127 void
00128 RhtXNode::getNodes(std::vector< std::vector< int > > *rv, int min_votes)
00129 {
00130 if (left) {
00131 ((RhtXNode*)left)->getNodes(rv, min_votes);
00132 }
00133
00134 if (y_root) {
00135 y_root->getNodes(rv, min_votes, x);
00136 }
00137
00138 if (right) {
00139 ((RhtXNode*)right)->getNodes(rv, min_votes);
00140 }
00141 }
00142
00143
00144
00145
00146 void
00147 RhtXNode::dump(std::ostream& s)
00148 {
00149 if (left)
00150 ((RhtXNode*)left)->dump(s);
00151 y_root->dump(s, x);
00152 if (right)
00153 ((RhtXNode*)right)->dump(s);
00154 }
00155
00156
00157
00158
00159
00160 RhtXNode *
00161 RhtXNode::generate(int x)
00162 {
00163 if (reuse_tail == NULL)
00164 {
00165 RhtXNode* p=new RhtXNode(x);
00166 p->next = reuse_head;
00167 reuse_head = p;
00168 return reuse_head;
00169 }
00170 else
00171 {
00172 RhtXNode* p=reuse_tail;
00173 reuse_tail = (RhtXNode*)(reuse_tail->next);
00174 p->clear(x);
00175 return p;
00176 }
00177 }
00178
00179
00180
00181
00182
00183 void
00184 RhtXNode::clear(int x)
00185 {
00186 RhtAccNode::clear(x);
00187 this->x = x;
00188 y_root = NULL;
00189 }
00190
00191
00192 void
00193 RhtXNode::reset(void)
00194 {
00195 reuse_tail = reuse_head;
00196 }
00197
00198
00199
00200 void
00201 RhtXNode::cleanup(void)
00202 {
00203 while(reuse_head)
00204 {
00205 reuse_tail = (RhtXNode*)reuse_head->next;
00206 delete reuse_head;
00207 reuse_head = reuse_tail;
00208 }
00209 }
00210
00211
00212
00213
00214
00215 RhtYNode::RhtYNode(int y)
00216 : RhtAccNode()
00217 {
00218 this->y=y;
00219 r_root = NULL;
00220 }
00221
00222
00223
00224
00225
00226
00227 int
00228 RhtYNode::insert(int y0, int r0)
00229 {
00230 if (y == y0)
00231 {
00232 if (!r_root)
00233 r_root = RhtRNode::generate(r0);
00234 return r_root->insert(r0);
00235 }
00236 else if (y0 < y)
00237 {
00238 if (!left)
00239 left = generate(y0);
00240 return ((RhtYNode*)left)->insert(y0, r0);
00241 }
00242 else
00243 {
00244 if (!right)
00245 right = generate(y0);
00246 return ((RhtYNode*)right)->insert(y0, r0);
00247 }
00248 }
00249
00250
00251
00252
00253
00254
00255 void
00256 RhtYNode::getNodes(std::vector< std::vector< int > > *rv, int min_votes, int x)
00257 {
00258 if (left) {
00259 ((RhtYNode*)left)->getNodes(rv, min_votes, x);
00260 }
00261
00262 if (r_root) {
00263 r_root->getNodes(rv, min_votes, x, y);
00264 }
00265
00266 if (right) {
00267 ((RhtYNode*)right)->getNodes(rv, min_votes, x);
00268 }
00269 }
00270
00271
00272
00273
00274
00275
00276 void
00277 RhtYNode::dump(std::ostream& s, int x)
00278 {
00279 if (left)
00280 ((RhtYNode*)left)->dump(s, x);
00281 r_root->dump(s, x, y);
00282 if (right)
00283 ((RhtYNode*)right)->dump(s, x);
00284 }
00285
00286
00287
00288
00289
00290
00291 RhtYNode *
00292 RhtYNode::generate(int y)
00293 {
00294 if (reuse_tail == NULL)
00295 {
00296 RhtYNode* p=new RhtYNode(y);
00297 p->next = reuse_head;
00298 reuse_head = p;
00299 return reuse_head;
00300 }
00301 else
00302 {
00303 RhtYNode* p=reuse_tail;
00304 reuse_tail = (RhtYNode*)(reuse_tail->next);
00305 p->clear(y);
00306 return p;
00307 }
00308 }
00309
00310
00311
00312
00313
00314 void
00315 RhtYNode::clear(int y)
00316 {
00317 RhtAccNode::clear(y);
00318 this->y = y;
00319 r_root = NULL;
00320 }
00321
00322
00323 void
00324 RhtYNode::reset(void)
00325 {
00326 reuse_tail = reuse_head;
00327 }
00328
00329
00330
00331 void
00332 RhtYNode::cleanup(void)
00333 {
00334 while(reuse_head)
00335 {
00336 reuse_tail = (RhtYNode*)reuse_head->next;
00337 delete reuse_head;
00338 reuse_head = reuse_tail;
00339 }
00340 }
00341
00342
00343
00344
00345 RhtRNode::RhtRNode(int r)
00346 : RhtAccNode()
00347 {
00348 this->r=r; count = 0;
00349 }
00350
00351
00352
00353 void
00354 RhtRNode::clear(void)
00355 {
00356 count = 0;
00357 }
00358
00359
00360
00361
00362
00363
00364
00365 int RhtRNode::insert(int r0)
00366 {
00367 if (r == r0)
00368 {
00369 return ++count;
00370 }
00371 else if (r0 < r)
00372 {
00373 if (!left)
00374 left = generate(r0);
00375 return ((RhtRNode*)left)->insert(r0);
00376 }
00377 else
00378 {
00379 if (!right)
00380 right = generate(r0);
00381 return ((RhtRNode*)right)->insert(r0);
00382 }
00383 }
00384
00385
00386
00387
00388
00389
00390
00391 void
00392 RhtRNode::getNodes(std::vector< std::vector< int > > *rv, int min_votes, int x, int y)
00393 {
00394 if (left) {
00395 ((RhtRNode*)left)->getNodes(rv, min_votes, x, y);
00396 }
00397 if (count >= min_votes) {
00398 vector< int > node;
00399 node.push_back( x );
00400 node.push_back( y );
00401 node.push_back( r );
00402 node.push_back( count );
00403 rv->push_back( node );
00404 }
00405 if (right) {
00406 ((RhtRNode*)right)->getNodes(rv, min_votes, x, y);
00407 }
00408 }
00409
00410
00411
00412
00413
00414
00415
00416 void
00417 RhtRNode::dump(std::ostream& s, int x, int y)
00418 {
00419 if (left)
00420 ((RhtRNode*)left)->dump(s, x, y);
00421 s << "("<<x<<","<<y<<","<<r<<") with vote "<<count<<endl;
00422 if (right)
00423 ((RhtRNode*)right)->dump(s, x, y);
00424 }
00425
00426
00427
00428
00429
00430
00431 RhtRNode *
00432 RhtRNode::generate(int r)
00433 {
00434 if (reuse_tail == NULL)
00435 {
00436 RhtRNode* p=new RhtRNode(r);
00437 p->next = reuse_head;
00438 reuse_head = p;
00439 return reuse_head;
00440 }
00441 else
00442 {
00443 RhtRNode* p=reuse_tail;
00444 reuse_tail = (RhtRNode*)(reuse_tail->next);
00445 p->clear(r);
00446 return p;
00447 }
00448 }
00449
00450
00451
00452
00453 void
00454 RhtRNode::clear(int r)
00455 {
00456 RhtAccNode::clear(r);
00457 this->r = r;
00458 count = 0;
00459 }
00460
00461
00462 void
00463 RhtRNode::reset(void)
00464 {
00465 reuse_tail = reuse_head;
00466 }
00467
00468
00469 void
00470 RhtRNode::cleanup(void)
00471 {
00472 while(reuse_head)
00473 {
00474 reuse_tail = (RhtRNode*)reuse_head->next;
00475 delete reuse_head;
00476 reuse_head = reuse_tail;
00477 }
00478 }
00479
00480
00481
00482 RhtAccumulator::RhtAccumulator()
00483 {
00484 root = NULL;
00485 max=0;
00486 }
00487
00488
00489
00490 RhtAccumulator::~RhtAccumulator()
00491 {
00492 RhtXNode::cleanup();
00493 RhtYNode::cleanup();
00494 RhtRNode::cleanup();
00495 }
00496
00497
00498
00499 void
00500 RhtAccumulator::reset(void)
00501 {
00502 max = 0;
00503 root = NULL;
00504 num_votes = 0;
00505 RhtXNode::reset();
00506 RhtYNode::reset();
00507 RhtRNode::reset();
00508 }
00509
00510
00511
00512
00513
00514
00515
00516
00517 int
00518 RhtAccumulator::accumulate(int x, int y, int r)
00519 {
00520 ++num_votes;
00521
00522 if (!root)
00523 root = RhtXNode::generate(x);
00524 int count = root->insert(x, y, r);
00525 if (count > max) {
00526 max = count;
00527 x_max = x;
00528 y_max = y;
00529 r_max = r;
00530 }
00531 return count;
00532 }
00533
00534
00535
00536
00537
00538
00539
00540
00541 int
00542 RhtAccumulator::getMax(int &x, int &y, int &r) const
00543 {
00544 x = x_max;
00545 y = y_max;
00546 r = r_max;
00547 return max;
00548 }
00549
00550
00551
00552
00553 void
00554 RhtAccumulator::dump(std::ostream& s)
00555 {
00556 if (root)
00557 root->dump(s);
00558 }
00559
00560
00561
00562
00563
00564 unsigned int
00565 RhtAccumulator::getNumVotes() const
00566 {
00567 return num_votes;
00568 }
00569
00570
00571
00572
00573
00574
00575 vector< vector< int > > *
00576 RhtAccumulator::getNodes(int min_votes)
00577 {
00578 vector< vector< int > > *rv = new vector< vector< int > >();
00579
00580 if ( (min_votes <= num_votes) && (root != NULL) ) {
00581 root->getNodes( rv, min_votes );
00582 }
00583
00584 return rv;
00585 }
00586
00587 }