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     image->draw_line(2, r, g, b, x1, y1, x2, y2);
  74     image->draw_line(2, r, g, b, x2, y2, x3, y3);
  75     image->draw_line(2, r, g, b, x3, y3, x4, y4);
  76     image->draw_line(2, r, g, b, x4, y4, x1, y1);
  77   }
  78 }
  79
  80 void PiReferential::draw_window(RGBImage *image,
  81                                 int registration_mode, Rectangle *window,
  82                                 int filled) {
  83   int r, g, b;
  84
  85   switch(registration_mode) {
  86
  87   case PiReferential::RM_HEAD:
  88     r = 0; g = 255; b = 0;
  89     break;
  90
  91   case PiReferential::RM_HEAD_NO_POLARITY:
  92     r = 128; g = 255; b = 128;
  93     break;
  94
  95   case PiReferential::RM_BELLY:
  96     r = 64; g = 0; b = 255;
  97     break;
  98
  99   case PiReferential::RM_BELLY_NO_POLARITY:
 100     r = 192; g = 128; b = 255;
 101     break;
 102
 103   case PiReferential::RM_HEAD_BELLY:
 104   case PiReferential::RM_HEAD_BELLY_EDGES:
 105     r = 255; g = 0; b = 0;
 106     break;
 107
 108   default:
 109     cerr << "INCONSISTENCY" << endl;
 110     abort();
 111   }
 112
 113   int xmin = int(window->xmin);
 114   int ymin = int(window->ymin);
 115   int xmax = int(window->xmax);
 116   int ymax = int(window->ymax);
 117
 118   if(global.pictures_for_article) {
 119     r = 255; g = 255; b = 255;
 120     image->draw_line(6, r, g, b, xmin, ymin, xmax, ymin);
 121     image->draw_line(6, r, g, b, xmax, ymin, xmax, ymax);
 122     image->draw_line(6, r, g, b, xmax, ymax, xmin, ymax);
 123     image->draw_line(6, r, g, b, xmin, ymax, xmin, ymin);
 124
 125     r =   0; g =   0; b =   0;
 126     image->draw_line(2, r, g, b, xmin, ymin, xmax, ymin);
 127     image->draw_line(2, r, g, b, xmax, ymin, xmax, ymax);
 128     image->draw_line(2, r, g, b, xmax, ymax, xmin, ymax);
 129     image->draw_line(2, r, g, b, xmin, ymax, xmin, ymin);
 130
 131   } else {
 132     image->draw_line(2, r, g, b, xmin, ymin, xmax, ymin);
 133     image->draw_line(2, r, g, b, xmax, ymin, xmax, ymax);
 134     image->draw_line(2, r, g, b, xmax, ymax, xmin, ymax);
 135     image->draw_line(2, r, g, b, xmin, ymax, xmin, ymin);
 136     if(filled) {
 137       int delta = 4;
 138       for(int d = ymin - ymax; d <= xmax - xmin; d += delta) {
 139         int x1 = xmin + d;
 140         int y1 = ymin;
 141         int x2 = xmin + d + ymax - ymin;
 142         int y2 = ymax;
 143         if(x1 < xmin) { y1 = y1 + (xmin - x1); x1 = xmin; }
 144         if(x2 > xmax) { y2 = y2 - (x2 - xmax); x2 = xmax; }
 145         image->draw_line(1, r, g, b, x1, y1, x2, y2);
 146       }
 147     }
 148   }
 149
 150 }
 151
 152 void PiReferential::draw_edge_and_scale(RGBImage *image,
 153                                         int registration_mode, Rectangle *window,
 154                                         int _tag, int _edge_scale) {
 155   const int ref_radius = 10;
 156   int r, g, b;
 157   int edges = 0;
 158
 159   switch(registration_mode) {
 160
 161   case PiReferential::RM_HEAD:
 162     r = 0; g = 255; b = 0;
 163     break;
 164
 165   case PiReferential::RM_HEAD_NO_POLARITY:
 166     r = 128; g = 255; b = 128;
 167     break;
 168
 169   case PiReferential::RM_BELLY:
 170     r = 64; g = 0; b = 255;
 171     break;
 172
 173   case PiReferential::RM_BELLY_NO_POLARITY:
 174     r = 192; g = 128; b = 255;
 175     break;
 176
 177   case PiReferential::RM_HEAD_BELLY_EDGES:
 178     edges = 1;
 179   case PiReferential::RM_HEAD_BELLY:
 180     r = 255; g = 0; b = 0;
 181     break;
 182
 183   default:
 184     cerr << "INCONSISTENCY" << endl;
 185     abort();
 186   }
 187
 188   scalar_t xc = (window->xmin + window->xmax)/2;
 189   scalar_t yc = (window->ymin + window->ymax)/2;
 190   int radius = ref_radius * (1 << _edge_scale);
 191
 192   image->draw_ellipse(1, r, g, b, xc, yc, radius, radius, 0);
 193
 194   if(_tag >= RichImage::first_edge_tag && _tag < RichImage::first_edge_tag + RichImage::nb_edge_tags) {
 195
 196     scalar_t dx, dy;
 197
 198     switch(_tag - RichImage::first_edge_tag) {
 199     case 0:
 200       dx =  0; dy = -1;
 201       break;
 202
 203     case 1:
 204       dx =  1; dy = -1;
 205       break;
 206
 207     case 2:
 208       dx =  1; dy =  0;
 209       break;
 210
 211     case 3:
 212       dx =  1; dy =  1;
 213       break;
 214
 215     case 4:
 216       dx =  0; dy =  1;
 217       break;
 218
 219     case 5:
 220       dx = -1; dy =  1;
 221       break;
 222
 223     case 6:
 224       dx = -1; dy =  0;
 225       break;
 226
 227     case 7:
 228       dx = -1; dy = -1;
 229       break;
 230
 231     default:
 232       abort();
 233     }
 234
 235     scalar_t l = sqrt(dx * dx + dy * dy);
 236
 237     if(edges) {
 238       int delta = 3;
 239       image->draw_ellipse(1, r, g, b, xc, yc, radius + delta, radius + delta, 0);
 240     }
 241
 242     for(scalar_t u = 0; u <= radius; u += 0.1) {
 243       scalar_t s = sqrt(radius * radius - (u * u * l * l))/l;
 244       image->draw_line(2, r, g, b,
 245                        int(xc + u * dx - s * dy), int(yc + u * dy + s * dx),
 246                        int(xc + u * dx + s * dy), int(yc + u * dy - s * dx));
 247     }
 248
 249   }
 250
 251   else if(_tag == RichImage::variance_tag) {
 252     image->draw_ellipse(1, r, g, b, xc, yc, 8, 8, 0);
 253   }
 254 }
 255
 256 PiReferential::PiReferential(PoseCell *cell) {
 257   scalar_t head_radius = sqrt(scalar_t(cell->_head_radius.min * cell->_head_radius.max));
 258
 259   _common_scale = global.scale_to_discrete_log_scale(head_radius / global.min_head_radius);
 260
 261   scalar_t discrete_scale_ratio = global.discrete_log_scale_to_scale(_common_scale);
 262
 263   //////////////////////////////////////////////////////////////////////
 264   // Locations and scales
 265
 266   // Head location
 267
 268   _head_xc = cell->_head_xc.middle() * discrete_scale_ratio;
 269   _head_yc = cell->_head_yc.middle() * discrete_scale_ratio;
 270   _head_radius = cell->_head_radius.middle() * discrete_scale_ratio;
 271   _head_window_scaling = _head_radius * 2.0;
 272
 273   // Body location
 274
 275   _body_xc = cell->_belly_xc.middle() * discrete_scale_ratio;
 276   _body_yc = cell->_belly_yc.middle() * discrete_scale_ratio;
 277
 278   _body_tilt = 0;
 279
 280   if((_head_xc - _body_xc) * cos(_body_tilt) + (_head_yc - _body_yc) * sin(_body_tilt) > 0) {
 281     _body_tilt += M_PI;
 282   }
 283
 284   // Belly location
 285
 286   const scalar_t belly_frame_factor = 2.0;
 287
 288   _belly_xc = cell->_belly_xc.middle() * discrete_scale_ratio;
 289   _belly_yc = cell->_belly_yc.middle() * discrete_scale_ratio;
 290   _belly_window_scaling = _head_window_scaling * belly_frame_factor;
 291
 292   // Head-belly location
 293
 294   _head_belly_xc = (_head_xc + _belly_xc) * 0.5;
 295   _head_belly_yc = (_head_yc + _belly_yc) * 0.5;
 296
 297   //////////////////////////////////////////////////////////////////////
 298   // Frames
 299
 300   if(_body_xc >= _head_xc) {
 301 //   if(_belly_xc >= _head_xc) {
 302     _horizontal_polarity = 1;
 303   } else {
 304     _horizontal_polarity = -1;
 305   }
 306
 307   // Head frame
 308
 309   if(_horizontal_polarity < 0) {
 310     _head_ux = _head_radius * 2.0;
 311     _head_uy = 0;
 312   } else {
 313     _head_ux = - _head_radius * 2.0;
 314     _head_uy = 0;
 315   }
 316
 317   _head_vx = 0;
 318   _head_vy = - _head_radius * 2.0;
 319
 320   _head_ux_nopolarity = _head_radius * 2.0;
 321   _head_uy_nopolarity = 0;
 322   _head_vx_nopolarity = 0;
 323   _head_vy_nopolarity = - _head_radius * 2.0;
 324
 325   // Belly frame
 326
 327   _belly_ux = _head_ux * belly_frame_factor;
 328   _belly_uy = _head_uy * belly_frame_factor;
 329   _belly_vx = _head_vx * belly_frame_factor;
 330   _belly_vy = _head_vy * belly_frame_factor;
 331
 332   _belly_ux_nopolarity = _head_ux_nopolarity * belly_frame_factor;
 333   _belly_uy_nopolarity = _head_uy_nopolarity * belly_frame_factor;
 334   _belly_vx_nopolarity = _head_vx_nopolarity * belly_frame_factor;
 335   _belly_vy_nopolarity = _head_vy_nopolarity * belly_frame_factor;
 336
 337   // Head-belly frame
 338
 339   _head_belly_ux = 2 * (_head_xc - _head_belly_xc);
 340   _head_belly_uy = 2 * (_head_yc - _head_belly_yc);
 341
 342   if(_horizontal_polarity < 0) {
 343     _head_belly_vx =   _head_belly_uy;
 344     _head_belly_vy = - _head_belly_ux;
 345   } else {
 346     _head_belly_vx = - _head_belly_uy;
 347     _head_belly_vy =   _head_belly_ux;
 348   }
 349
 350   scalar_t l = sqrt(_head_belly_vx * _head_belly_vx + _head_belly_vy * _head_belly_vy);
 351
 352   _head_belly_vx = (_head_belly_vx / l) * _head_radius * 2;
 353   _head_belly_vy = (_head_belly_vy / l) * _head_radius * 2;
 354   _head_belly_edge_shift = int(floor(- RichImage::nb_edge_tags * atan2(_head_belly_ux, _head_belly_uy) / (2 * M_PI) + 0.5));
 355   _head_belly_edge_shift = (RichImage::nb_edge_tags + _head_belly_edge_shift) % RichImage::nb_edge_tags;
 356 }
 357
 358 int PiReferential::common_scale() {
 359   return _common_scale;
 360 }
 361
 362 void PiReferential::register_rectangle(int registration_mode,
 363                                        Rectangle *original,
 364                                        Rectangle *result) {
 365   scalar_t alpha, beta , xc, yc, w, h;
 366
 367   alpha = (original->xmin + original->xmax) * 0.5;
 368   beta  = (original->ymin + original->ymax) * 0.5;
 369
 370   switch(registration_mode) {
 371
 372   case RM_HEAD:
 373     {
 374       xc = _head_xc + alpha * _head_ux + beta * _head_vx;
 375       yc = _head_yc + alpha * _head_uy + beta * _head_vy;
 376       w = (original->xmax - original->xmin) * _head_window_scaling;
 377       h = (original->ymax - original->ymin) * _head_window_scaling;
 378     }
 379     break;
 380
 381   case RM_HEAD_NO_POLARITY:
 382     {
 383       xc = _head_xc + alpha * _head_ux_nopolarity + beta * _head_vx_nopolarity;
 384       yc = _head_yc + alpha * _head_uy_nopolarity + beta * _head_vy_nopolarity;
 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_BELLY:
 391     {
 392       xc = _belly_xc + alpha * _belly_ux + beta * _belly_vx;
 393       yc = _belly_yc + alpha * _belly_uy + beta * _belly_vy;
 394       w = (original->xmax - original->xmin) * _belly_window_scaling;
 395       h = (original->ymax - original->ymin) * _belly_window_scaling;
 396     }
 397     break;
 398
 399   case RM_BELLY_NO_POLARITY:
 400     {
 401       xc = _belly_xc + alpha * _belly_ux_nopolarity + beta * _belly_vx_nopolarity;
 402       yc = _belly_yc + alpha * _belly_uy_nopolarity + beta * _belly_vy_nopolarity;
 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_HEAD_BELLY:
 409   case RM_HEAD_BELLY_EDGES:
 410     {
 411       xc = _head_belly_xc + alpha * _head_belly_ux + beta * _head_belly_vx;
 412       yc = _head_belly_yc + alpha * _head_belly_uy + beta * _head_belly_vy;
 413       w = (original->xmax - original->xmin) * _head_window_scaling;
 414       h = (original->ymax - original->ymin) * _head_window_scaling;
 415     }
 416     break;
 417
 418   default:
 419     cerr << "Undefined registration mode." << endl;
 420     abort();
 421   }
 422
 423   result->xmin = xc - 0.5 * w;
 424   result->ymin = yc - 0.5 * h;
 425   result->xmax = xc + 0.5 * w;
 426   result->ymax = yc + 0.5 * h;
 427
 428   ASSERT(result->xmin < result->xmax && result->ymin < result->ymax);
 429 }
 430
 431 int PiReferential::register_edge(int registration_mode, int edge_type) {
 432
 433   if(edge_type >= RichImage::first_edge_tag &&
 434      edge_type < RichImage::first_edge_tag + RichImage::nb_edge_tags) {
 435
 436     int e = edge_type - RichImage::first_edge_tag;
 437
 438     switch(registration_mode) {
 439     case PiReferential::RM_HEAD_NO_POLARITY:
 440     case PiReferential::RM_BELLY_NO_POLARITY:
 441       break;
 442
 443     case PiReferential::RM_HEAD:
 444     case PiReferential::RM_BELLY:
 445     case PiReferential::RM_HEAD_BELLY:
 446       if(_horizontal_polarity < 0) {
 447         e = (RichImage::nb_edge_tags - e) % RichImage::nb_edge_tags;
 448       }
 449       break;
 450
 451     case PiReferential::RM_HEAD_BELLY_EDGES:
 452       if(_horizontal_polarity < 0) {
 453         e = (RichImage::nb_edge_tags - e) % RichImage::nb_edge_tags;
 454       }
 455       e += _head_belly_edge_shift;
 456       break;
 457
 458     default:
 459       cerr << "INCONSISTENCY" << endl;
 460       abort();
 461     }
 462
 463     e = e % RichImage::nb_edge_tags;
 464
 465     return RichImage::first_edge_tag + e;
 466
 467   }
 468
 469   else return edge_type;
 470 }
 471
 472 void PiReferential::draw(RGBImage *image, int level) {
 473   int x1, y1, x2, y2, x3, y3, x4, y4;
 474
 475   if(level >= 1) {
 476
 477     // Draw the RM_BELLY reference frame
 478
 479     x1 = int(_belly_xc + _belly_ux + _belly_vx);
 480     y1 = int(_belly_yc + _belly_uy + _belly_vy);
 481     x2 = int(_belly_xc - _belly_ux + _belly_vx);
 482     y2 = int(_belly_yc - _belly_uy + _belly_vy);
 483     x3 = int(_belly_xc - _belly_ux - _belly_vx);
 484     y3 = int(_belly_yc - _belly_uy - _belly_vy);
 485     x4 = int(_belly_xc + _belly_ux - _belly_vx);
 486     y4 = int(_belly_yc + _belly_uy - _belly_vy);
 487
 488     draw_frame(image, RM_BELLY, x1, y1, x2, y2, x3, y3, x4, y4);
 489
 490     // Draw the RM_HEAD_BELLY reference frame
 491
 492     x1 = int(_head_belly_xc + _head_belly_ux + _head_belly_vx);
 493     y1 = int(_head_belly_yc + _head_belly_uy + _head_belly_vy);
 494     x2 = int(_head_belly_xc - _head_belly_ux + _head_belly_vx);
 495     y2 = int(_head_belly_yc - _head_belly_uy + _head_belly_vy);
 496     x3 = int(_head_belly_xc - _head_belly_ux - _head_belly_vx);
 497     y3 = int(_head_belly_yc - _head_belly_uy - _head_belly_vy);
 498     x4 = int(_head_belly_xc + _head_belly_ux - _head_belly_vx);
 499     y4 = int(_head_belly_yc + _head_belly_uy - _head_belly_vy);
 500
 501     draw_frame(image, RM_HEAD_BELLY, x1, y1, x2, y2, x3, y3, x4, y4);
 502   }
 503
 504   // Draw the RM_HEAD reference frame
 505
 506   x1 = int(_head_xc + _head_ux + _head_vx);
 507   y1 = int(_head_yc + _head_uy + _head_vy);
 508   x2 = int(_head_xc - _head_ux + _head_vx);
 509   y2 = int(_head_yc - _head_uy + _head_vy);
 510   x3 = int(_head_xc - _head_ux - _head_vx);
 511   y3 = int(_head_yc - _head_uy - _head_vy);
 512   x4 = int(_head_xc + _head_ux - _head_vx);
 513   y4 = int(_head_yc + _head_uy - _head_vy);
 514
 515   draw_frame(image, RM_HEAD, x1, y1, x2, y2, x3, y3, x4, y4);
 516 }
 517
 518 void PiReferential::print_registration_mode(ostream *out, int registration_mode) {
 519   switch(registration_mode) {
 520   case RM_HEAD:
 521     (*out) << "RM_HEAD";
 522     break;
 523   case RM_HEAD_NO_POLARITY:
 524     (*out) << "RM_HEAD_NO_POLARITY";
 525     break;
 526   case RM_BELLY:
 527     (*out) << "RM_BELLY";
 528     break;
 529   case RM_BELLY_NO_POLARITY:
 530     (*out) << "RM_BELLY_NO_POLARITY";
 531     break;
 532   case RM_HEAD_BELLY:
 533     (*out) << "RM_HEAD_BELLY";
 534     break;
 535   case RM_HEAD_BELLY_EDGES:
 536     (*out) << "RM_HEAD_BELLY_EDGES";
 537     break;
 538   default:
 539     abort();
 540   }
 541 }