pi_referential.cc

   1
   2 ///////////////////////////////////////////////////////////////////////////
   3 // This program is free software: you can redistribute it and/or modify  //
   4 // it under the terms of the version 3 of the GNU General Public License //
   5 // as published by the Free Software Foundation.                         //
   6 //                                                                       //
   7 // This program is distributed in the hope that it will be useful, but   //
   8 // WITHOUT ANY WARRANTY; without even the implied warranty of            //
   9 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU      //
  10 // General Public License for more details.                              //
  11 //                                                                       //
  12 // You should have received a copy of the GNU General Public License     //
  13 // along with this program. If not, see <http://www.gnu.org/licenses/>.  //
  14 //                                                                       //
  15 // Written by Francois Fleuret, (C) IDIAP                                //
  16 // Contact <francois.fleuret@idiap.ch> for comments & bug reports        //
  17 ///////////////////////////////////////////////////////////////////////////
  18
  19 #include "pi_referential.h"
  20 #include "global.h"
  21 #include "rich_image.h"
  22
  23 void PiReferential::draw_frame(RGBImage *image,
  24                                int registration_mode,
  25                                int x1, int y1,
  26                                int x2, int y2,
  27                                int x3, int y3,
  28                                int x4, int y4) {
  29
  30   int r, g, b;
  31
  32   switch(registration_mode) {
  33
  34   case PiReferential::RM_HEAD:
  35     r = 0; g = 255; b = 0;
  36     break;
  37
  38   case PiReferential::RM_HEAD_NO_POLARITY:
  39     r = 128; g = 255; b = 128;
  40     break;
  41
  42   case PiReferential::RM_BELLY:
  43     r = 64; g = 0; b = 255;
  44     break;
  45
  46   case PiReferential::RM_BELLY_NO_POLARITY:
  47     r = 192; g = 128; b = 255;
  48     break;
  49
  50   case PiReferential::RM_HEAD_BELLY:
  51   case PiReferential::RM_HEAD_BELLY_EDGES:
  52     r = 255; g = 0; b = 0;
  53     break;
  54
  55   default:
  56     cerr << "INCONSISTENCY" << endl;
  57     abort();
  58   }
  59
  60   if(global.pictures_for_article) {
  61     r = 255; g = 255; b = 255;
  62     image->draw_line(6, r, g, b, x1, y1, x2, y2);
  63     image->draw_line(6, r, g, b, x2, y2, x3, y3);
  64     image->draw_line(6, r, g, b, x3, y3, x4, y4);
  65     image->draw_line(6, r, g, b, x4, y4, x1, y1);
  66
  67     r =   0; g =   0; b =   0;
  68     image->draw_line(2, r, g, b, x1, y1, x2, y2);
  69     image->draw_line(2, r, g, b, x2, y2, x3, y3);
  70     image->draw_line(2, r, g, b, x3, y3, x4, y4);
  71     image->draw_line(2, r, g, b, x4, y4, x1, y1);
  72   } else {
  73     //   int xc = (x1 + x2 + x3 + x4)/4, yc = (y1 + y2 + y3 + y4)/4;
  74     //     image->draw_line(1, r, g, b, xc - delta, yc, xc + delta, yc);
  75     //     image->draw_line(1, r, g, b, xc, yc - delta, xc, yc + delta);
  76     image->draw_line(2, r, g, b, x1, y1, x2, y2);
  77     image->draw_line(2, r, g, b, x2, y2, x3, y3);
  78     image->draw_line(2, r, g, b, x3, y3, x4, y4);
  79     image->draw_line(2, r, g, b, x4, y4, x1, y1);
  80     //     image->draw_line(2, r, g, b,
  81     //                      (2*xc + 5 * x1 + 5 * x2)/12, (2 * yc + 5 * y1 + 5 * y2)/12,
  82     //                      (x1 + x2)/2, (y1 + y2)/2);
  83     //     image->draw_line(6, r, g, b,
  84     //                      (2*xc + 3 * x2 + 3 * x3)/8, (2 * yc + 3 * y2 + 3 * y3)/8,
  85     //                      (x2 + x3)/2, (y2 + y3)/2
  86     //                      );
  87   }
  88 }
  89
  90 void PiReferential::draw_window(RGBImage *image,
  91                                 int registration_mode, Rectangle *window,
  92                                 int filled) {
  93   int r, g, b;
  94
  95   switch(registration_mode) {
  96
  97   case PiReferential::RM_HEAD:
  98     r = 0; g = 255; b = 0;
  99     break;
 100
 101   case PiReferential::RM_HEAD_NO_POLARITY:
 102     r = 128; g = 255; b = 128;
 103     break;
 104
 105   case PiReferential::RM_BELLY:
 106     r = 64; g = 0; b = 255;
 107     break;
 108
 109   case PiReferential::RM_BELLY_NO_POLARITY:
 110     r = 192; g = 128; b = 255;
 111     break;
 112
 113   case PiReferential::RM_HEAD_BELLY:
 114   case PiReferential::RM_HEAD_BELLY_EDGES:
 115     r = 255; g = 0; b = 0;
 116     break;
 117
 118   default:
 119     cerr << "INCONSISTENCY" << endl;
 120     abort();
 121   }
 122
 123   int xmin = int(window->xmin);
 124   int ymin = int(window->ymin);
 125   int xmax = int(window->xmax);
 126   int ymax = int(window->ymax);
 127
 128   if(global.pictures_for_article) {
 129     r = 255; g = 255; b = 255;
 130     image->draw_line(6, r, g, b, xmin, ymin, xmax, ymin);
 131     image->draw_line(6, r, g, b, xmax, ymin, xmax, ymax);
 132     image->draw_line(6, r, g, b, xmax, ymax, xmin, ymax);
 133     image->draw_line(6, r, g, b, xmin, ymax, xmin, ymin);
 134
 135     r =   0; g =   0; b =   0;
 136     image->draw_line(2, r, g, b, xmin, ymin, xmax, ymin);
 137     image->draw_line(2, r, g, b, xmax, ymin, xmax, ymax);
 138     image->draw_line(2, r, g, b, xmax, ymax, xmin, ymax);
 139     image->draw_line(2, r, g, b, xmin, ymax, xmin, ymin);
 140
 141   } else {
 142     image->draw_line(2, r, g, b, xmin, ymin, xmax, ymin);
 143     image->draw_line(2, r, g, b, xmax, ymin, xmax, ymax);
 144     image->draw_line(2, r, g, b, xmax, ymax, xmin, ymax);
 145     image->draw_line(2, r, g, b, xmin, ymax, xmin, ymin);
 146     if(filled) {
 147       int delta = 4;
 148       for(int d = ymin - ymax; d <= xmax - xmin; d += delta) {
 149         int x1 = xmin + d;
 150         int y1 = ymin;
 151         int x2 = xmin + d + ymax - ymin;
 152         int y2 = ymax;
 153         if(x1 < xmin) { y1 = y1 + (xmin - x1); x1 = xmin; }
 154         if(x2 > xmax) { y2 = y2 - (x2 - xmax); x2 = xmax; }
 155         image->draw_line(1, r, g, b, x1, y1, x2, y2);
 156       }
 157     }
 158   }
 159
 160 }
 161
 162 void PiReferential::draw_edge_and_scale(RGBImage *image,
 163                                         int registration_mode, Rectangle *window,
 164                                         int _tag, int _edge_scale) {
 165   const int ref_radius = 10;
 166   int r, g, b;
 167   int edges = 0;
 168
 169   switch(registration_mode) {
 170
 171   case PiReferential::RM_HEAD:
 172     r = 0; g = 255; b = 0;
 173     break;
 174
 175   case PiReferential::RM_HEAD_NO_POLARITY:
 176     r = 128; g = 255; b = 128;
 177     break;
 178
 179   case PiReferential::RM_BELLY:
 180     r = 64; g = 0; b = 255;
 181     break;
 182
 183   case PiReferential::RM_BELLY_NO_POLARITY:
 184     r = 192; g = 128; b = 255;
 185     break;
 186
 187   case PiReferential::RM_HEAD_BELLY_EDGES:
 188     edges = 1;
 189   case PiReferential::RM_HEAD_BELLY:
 190     r = 255; g = 0; b = 0;
 191     break;
 192
 193   default:
 194     cerr << "INCONSISTENCY" << endl;
 195     abort();
 196   }
 197
 198   scalar_t xc = (window->xmin + window->xmax)/2;
 199   scalar_t yc = (window->ymin + window->ymax)/2;
 200   int radius = ref_radius * (1 << _edge_scale);
 201
 202   image->draw_ellipse(1, r, g, b, xc, yc, radius, radius, 0);
 203
 204   if(_tag >= RichImage::first_edge_tag && _tag < RichImage::first_edge_tag + RichImage::nb_edge_tags) {
 205
 206     scalar_t dx, dy;
 207
 208     switch(_tag - RichImage::first_edge_tag) {
 209     case 0:
 210       dx =  0; dy = -1;
 211       break;
 212
 213     case 1:
 214       dx =  1; dy = -1;
 215       break;
 216
 217     case 2:
 218       dx =  1; dy =  0;
 219       break;
 220
 221     case 3:
 222       dx =  1; dy =  1;
 223       break;
 224
 225     case 4:
 226       dx =  0; dy =  1;
 227       break;
 228
 229     case 5:
 230       dx = -1; dy =  1;
 231       break;
 232
 233     case 6:
 234       dx = -1; dy =  0;
 235       break;
 236
 237     case 7:
 238       dx = -1; dy = -1;
 239       break;
 240
 241     default:
 242       abort();
 243     }
 244
 245     scalar_t l = sqrt(dx * dx + dy * dy);
 246
 247     if(edges) {
 248       int delta = 3;
 249       image->draw_ellipse(1, r, g, b, xc, yc, radius + delta, radius + delta, 0);
 250     }
 251
 252     for(scalar_t u = 0; u <= radius; u += 0.1) {
 253       scalar_t s = sqrt(radius * radius - (u * u * l * l))/l;
 254       image->draw_line(2, r, g, b,
 255                        int(xc + u * dx - s * dy), int(yc + u * dy + s * dx),
 256                        int(xc + u * dx + s * dy), int(yc + u * dy - s * dx));
 257     }
 258
 259   }
 260
 261   else if(_tag == RichImage::variance_tag) {
 262     image->draw_ellipse(1, r, g, b, xc, yc, 8, 8, 0);
 263   }
 264 }
 265
 266 PiReferential::PiReferential(PoseCell *cell) {
 267   scalar_t head_radius = sqrt(scalar_t(cell->_head_radius.min * cell->_head_radius.max));
 268
 269   _common_scale = global.scale_to_discrete_log_scale(head_radius / global.min_head_radius);
 270
 271   scalar_t discrete_scale_ratio = global.discrete_log_scale_to_scale(_common_scale);
 272
 273   //////////////////////////////////////////////////////////////////////
 274   // Locations and scales
 275
 276   // Head location
 277
 278   _head_xc = cell->_head_xc.middle() * discrete_scale_ratio;
 279   _head_yc = cell->_head_yc.middle() * discrete_scale_ratio;
 280   _head_radius = cell->_head_radius.middle() * discrete_scale_ratio;
 281   _head_window_scaling = _head_radius * 2.0;
 282
 283   // Body location
 284
 285   _body_xc = cell->_belly_xc.middle() * discrete_scale_ratio;
 286   _body_yc = cell->_belly_yc.middle() * discrete_scale_ratio;
 287
 288   _body_tilt = 0;
 289
 290   if((_head_xc - _body_xc) * cos(_body_tilt) + (_head_yc - _body_yc) * sin(_body_tilt) > 0) {
 291     _body_tilt += M_PI;
 292   }
 293
 294   // Belly location
 295
 296   const scalar_t belly_frame_factor = 2.0;
 297
 298   _belly_xc = cell->_belly_xc.middle() * discrete_scale_ratio;
 299   _belly_yc = cell->_belly_yc.middle() * discrete_scale_ratio;
 300   _belly_window_scaling = _head_window_scaling * belly_frame_factor;
 301
 302   // Head-belly location
 303
 304   _head_belly_xc = (_head_xc + _belly_xc) * 0.5;
 305   _head_belly_yc = (_head_yc + _belly_yc) * 0.5;
 306
 307   //////////////////////////////////////////////////////////////////////
 308   // Frames
 309
 310   if(_body_xc >= _head_xc) {
 311     _horizontal_polarity = 1;
 312   } else {
 313     _horizontal_polarity = -1;
 314   }
 315
 316   // Head frame
 317
 318   if(_horizontal_polarity < 0) {
 319     _head_ux = _head_radius * 2.0;
 320     _head_uy = 0;
 321   } else {
 322     _head_ux = - _head_radius * 2.0;
 323     _head_uy = 0;
 324   }
 325
 326   _head_vx = 0;
 327   _head_vy = - _head_radius * 2.0;
 328
 329   _head_ux_nopolarity = _head_radius * 2.0;
 330   _head_uy_nopolarity = 0;
 331   _head_vx_nopolarity = 0;
 332   _head_vy_nopolarity = - _head_radius * 2.0;
 333
 334   // Belly frame
 335
 336   _belly_ux = _head_ux * belly_frame_factor;
 337   _belly_uy = _head_uy * belly_frame_factor;
 338   _belly_vx = _head_vx * belly_frame_factor;
 339   _belly_vy = _head_vy * belly_frame_factor;
 340
 341   _belly_ux_nopolarity = _head_ux_nopolarity * belly_frame_factor;
 342   _belly_uy_nopolarity = _head_uy_nopolarity * belly_frame_factor;
 343   _belly_vx_nopolarity = _head_vx_nopolarity * belly_frame_factor;
 344   _belly_vy_nopolarity = _head_vy_nopolarity * belly_frame_factor;
 345
 346   // Head-belly frame
 347
 348   _head_belly_ux = 2 * (_head_xc - _head_belly_xc);
 349   _head_belly_uy = 2 * (_head_yc - _head_belly_yc);
 350
 351   if(_horizontal_polarity < 0) {
 352     _head_belly_vx =   _head_belly_uy;
 353     _head_belly_vy = - _head_belly_ux;
 354   } else {
 355     _head_belly_vx = - _head_belly_uy;
 356     _head_belly_vy =   _head_belly_ux;
 357   }
 358
 359   scalar_t l = sqrt(_head_belly_vx * _head_belly_vx + _head_belly_vy * _head_belly_vy);
 360
 361   _head_belly_vx = _head_belly_vx/l * _head_radius * 2;
 362   _head_belly_vy = _head_belly_vy/l * _head_radius * 2;
 363   _head_belly_edge_shift = int(floor(- RichImage::nb_edge_tags * atan2(_head_belly_ux, _head_belly_uy) / (2 * M_PI) + 0.5));
 364   _head_belly_edge_shift = (RichImage::nb_edge_tags + _head_belly_edge_shift) % RichImage::nb_edge_tags;
 365 }
 366
 367 int PiReferential::common_scale() {
 368   return _common_scale;
 369 }
 370
 371 void PiReferential::register_rectangle(int registration_mode,
 372                                        Rectangle *original,
 373                                        Rectangle *result) {
 374   scalar_t alpha, beta , xc, yc, w, h;
 375
 376   alpha = (original->xmin + original->xmax) * 0.5;
 377   beta  = (original->ymin + original->ymax) * 0.5;
 378
 379   switch(registration_mode) {
 380
 381   case RM_HEAD:
 382     {
 383       xc = _head_xc + alpha * _head_ux + beta * _head_vx;
 384       yc = _head_yc + alpha * _head_uy + beta * _head_vy;
 385       w = (original->xmax - original->xmin) * _head_window_scaling;
 386       h = (original->ymax - original->ymin) * _head_window_scaling;
 387     }
 388     break;
 389
 390   case RM_HEAD_NO_POLARITY:
 391     {
 392       xc = _head_xc + alpha * _head_ux_nopolarity + beta * _head_vx_nopolarity;
 393       yc = _head_yc + alpha * _head_uy_nopolarity + beta * _head_vy_nopolarity;
 394       w = (original->xmax - original->xmin) * _head_window_scaling;
 395       h = (original->ymax - original->ymin) * _head_window_scaling;
 396     }
 397     break;
 398
 399   case RM_BELLY:
 400     {
 401       xc = _belly_xc + alpha * _belly_ux + beta * _belly_vx;
 402       yc = _belly_yc + alpha * _belly_uy + beta * _belly_vy;
 403       w = (original->xmax - original->xmin) * _belly_window_scaling;
 404       h = (original->ymax - original->ymin) * _belly_window_scaling;
 405     }
 406     break;
 407
 408   case RM_BELLY_NO_POLARITY:
 409     {
 410       xc = _belly_xc + alpha * _belly_ux_nopolarity + beta * _belly_vx_nopolarity;
 411       yc = _belly_yc + alpha * _belly_uy_nopolarity + beta * _belly_vy_nopolarity;
 412       w = (original->xmax - original->xmin) * _belly_window_scaling;
 413       h = (original->ymax - original->ymin) * _belly_window_scaling;
 414     }
 415     break;
 416
 417   case RM_HEAD_BELLY:
 418   case RM_HEAD_BELLY_EDGES:
 419     {
 420       xc = _head_belly_xc + alpha * _head_belly_ux + beta * _head_belly_vx;
 421       yc = _head_belly_yc + alpha * _head_belly_uy + beta * _head_belly_vy;
 422       w = (original->xmax - original->xmin) * _head_window_scaling;
 423       h = (original->ymax - original->ymin) * _head_window_scaling;
 424     }
 425     break;
 426
 427   default:
 428     cerr << "Undefined registration mode." << endl;
 429     abort();
 430   }
 431
 432   result->xmin = xc - 0.5 * w;
 433   result->ymin = yc - 0.5 * h;
 434   result->xmax = xc + 0.5 * w;
 435   result->ymax = yc + 0.5 * h;
 436
 437   ASSERT(result->xmin < result->xmax && result->ymin < result->ymax);
 438 }
 439
 440 int PiReferential::register_edge(int registration_mode, int edge_type) {
 441
 442   if(edge_type >= RichImage::first_edge_tag &&
 443      edge_type < RichImage::first_edge_tag + RichImage::nb_edge_tags) {
 444
 445     int e = edge_type - RichImage::first_edge_tag;
 446
 447     switch(registration_mode) {
 448     case PiReferential::RM_HEAD_NO_POLARITY:
 449     case PiReferential::RM_BELLY_NO_POLARITY:
 450       break;
 451
 452     case PiReferential::RM_HEAD:
 453     case PiReferential::RM_BELLY:
 454     case PiReferential::RM_HEAD_BELLY:
 455       if(_horizontal_polarity < 0) {
 456         e = (RichImage::nb_edge_tags - e) % RichImage::nb_edge_tags;
 457       }
 458       break;
 459
 460     case PiReferential::RM_HEAD_BELLY_EDGES:
 461       if(_horizontal_polarity < 0) {
 462         e = (RichImage::nb_edge_tags - e) % RichImage::nb_edge_tags;
 463       }
 464       e += _head_belly_edge_shift;
 465       break;
 466
 467     default:
 468       cerr << "INCONSISTENCY" << endl;
 469       abort();
 470     }
 471
 472     e = e % RichImage::nb_edge_tags;
 473
 474     return RichImage::first_edge_tag + e;
 475
 476   }
 477
 478   else return edge_type;
 479 }
 480
 481 void PiReferential::draw(RGBImage *image, int level) {
 482   int x1, y1, x2, y2, x3, y3, x4, y4;
 483
 484   if(level >= 1) {
 485
 486     // Draw the RM_BELLY reference frame
 487
 488     x1 = int(_belly_xc + _belly_ux + _belly_vx);
 489     y1 = int(_belly_yc + _belly_uy + _belly_vy);
 490     x2 = int(_belly_xc - _belly_ux + _belly_vx);
 491     y2 = int(_belly_yc - _belly_uy + _belly_vy);
 492     x3 = int(_belly_xc - _belly_ux - _belly_vx);
 493     y3 = int(_belly_yc - _belly_uy - _belly_vy);
 494     x4 = int(_belly_xc + _belly_ux - _belly_vx);
 495     y4 = int(_belly_yc + _belly_uy - _belly_vy);
 496
 497     draw_frame(image, RM_BELLY, x1, y1, x2, y2, x3, y3, x4, y4);
 498
 499     // Draw the RM_HEAD_BELLY reference frame
 500
 501     x1 = int(_head_belly_xc + _head_belly_ux + _head_belly_vx);
 502     y1 = int(_head_belly_yc + _head_belly_uy + _head_belly_vy);
 503     x2 = int(_head_belly_xc - _head_belly_ux + _head_belly_vx);
 504     y2 = int(_head_belly_yc - _head_belly_uy + _head_belly_vy);
 505     x3 = int(_head_belly_xc - _head_belly_ux - _head_belly_vx);
 506     y3 = int(_head_belly_yc - _head_belly_uy - _head_belly_vy);
 507     x4 = int(_head_belly_xc + _head_belly_ux - _head_belly_vx);
 508     y4 = int(_head_belly_yc + _head_belly_uy - _head_belly_vy);
 509
 510     draw_frame(image, RM_HEAD_BELLY, x1, y1, x2, y2, x3, y3, x4, y4);
 511   }
 512
 513   // Draw the RM_HEAD reference frame
 514
 515   x1 = int(_head_xc + _head_ux + _head_vx);
 516   y1 = int(_head_yc + _head_uy + _head_vy);
 517   x2 = int(_head_xc - _head_ux + _head_vx);
 518   y2 = int(_head_yc - _head_uy + _head_vy);
 519   x3 = int(_head_xc - _head_ux - _head_vx);
 520   y3 = int(_head_yc - _head_uy - _head_vy);
 521   x4 = int(_head_xc + _head_ux - _head_vx);
 522   y4 = int(_head_yc + _head_uy - _head_vy);
 523
 524   draw_frame(image, RM_HEAD, x1, y1, x2, y2, x3, y3, x4, y4);
 525 }
 526
 527 void PiReferential::print_registration_mode(ostream *out, int registration_mode) {
 528   switch(registration_mode) {
 529   case RM_HEAD:
 530     (*out) << "RM_HEAD";
 531     break;
 532   case RM_HEAD_NO_POLARITY:
 533     (*out) << "RM_HEAD_NO_POLARITY";
 534     break;
 535   case RM_BELLY:
 536     (*out) << "RM_BELLY";
 537     break;
 538   case RM_BELLY_NO_POLARITY:
 539     (*out) << "RM_BELLY_NO_POLARITY";
 540     break;
 541   case RM_HEAD_BELLY:
 542     (*out) << "RM_HEAD_BELLY";
 543     break;
 544   case RM_HEAD_BELLY_EDGES:
 545     (*out) << "RM_HEAD_BELLY_EDGES";
 546     break;
 547   default:
 548     abort();
 549   }
 550 }