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 PiReferential::PiReferential(PoseCell *cell) {
 153   scalar_t head_radius = sqrt(scalar_t(cell->_head_radius.min * cell->_head_radius.max));
 154
 155   _common_scale = global.scale_to_discrete_log_scale(head_radius / global.min_head_radius);
 156
 157   scalar_t discrete_scale_ratio = global.discrete_log_scale_to_scale(_common_scale);
 158
 159   //////////////////////////////////////////////////////////////////////
 160   // Locations and scales
 161
 162   // Head location
 163
 164   _head_xc = cell->_head_xc.middle() * discrete_scale_ratio;
 165   _head_yc = cell->_head_yc.middle() * discrete_scale_ratio;
 166   _head_radius = cell->_head_radius.middle() * discrete_scale_ratio;
 167   _head_window_scaling = _head_radius * 2.0;
 168
 169   // Body location
 170
 171   // Useless code, but necessary to keep the exact same results with
 172   // g++ 4.1 and -O3 options on reference experiments.
 173
 174   _body_xc = cell->_belly_xc.middle() * discrete_scale_ratio;
 175   _body_yc = cell->_belly_yc.middle() * discrete_scale_ratio;
 176
 177   _body_tilt = 0;
 178
 179   if((_head_xc - _body_xc) * cos(_body_tilt) + (_head_yc - _body_yc) * sin(_body_tilt) > 0) {
 180     _body_tilt += M_PI;
 181   }
 182
 183   // Belly location
 184
 185   const scalar_t belly_frame_factor = 2.0;
 186
 187   _belly_xc = cell->_belly_xc.middle() * discrete_scale_ratio;
 188   _belly_yc = cell->_belly_yc.middle() * discrete_scale_ratio;
 189   _belly_window_scaling = _head_window_scaling * belly_frame_factor;
 190
 191   // Head-belly location
 192
 193   _head_belly_xc = (_head_xc + _belly_xc) * 0.5;
 194   _head_belly_yc = (_head_yc + _belly_yc) * 0.5;
 195
 196   //////////////////////////////////////////////////////////////////////
 197   // Frames
 198
 199   if(_body_xc >= _head_xc) {
 200   //   if(_belly_xc >= _head_xc) {
 201     _horizontal_polarity = 1;
 202   } else {
 203     _horizontal_polarity = -1;
 204   }
 205
 206   // Head frame
 207
 208   if(_horizontal_polarity < 0) {
 209     _head_ux = _head_radius * 2.0;
 210     _head_uy = 0;
 211   } else {
 212     _head_ux = - _head_radius * 2.0;
 213     _head_uy = 0;
 214   }
 215
 216   _head_vx = 0;
 217   _head_vy = - _head_radius * 2.0;
 218
 219   _head_ux_nopolarity = _head_radius * 2.0;
 220   _head_uy_nopolarity = 0;
 221   _head_vx_nopolarity = 0;
 222   _head_vy_nopolarity = - _head_radius * 2.0;
 223
 224   // Belly frame
 225
 226   _belly_ux = _head_ux * belly_frame_factor;
 227   _belly_uy = _head_uy * belly_frame_factor;
 228   _belly_vx = _head_vx * belly_frame_factor;
 229   _belly_vy = _head_vy * belly_frame_factor;
 230
 231   _belly_ux_nopolarity = _head_ux_nopolarity * belly_frame_factor;
 232   _belly_uy_nopolarity = _head_uy_nopolarity * belly_frame_factor;
 233   _belly_vx_nopolarity = _head_vx_nopolarity * belly_frame_factor;
 234   _belly_vy_nopolarity = _head_vy_nopolarity * belly_frame_factor;
 235
 236   // Head-belly frame
 237
 238   _head_belly_ux = 2 * (_head_xc - _head_belly_xc);
 239   _head_belly_uy = 2 * (_head_yc - _head_belly_yc);
 240
 241   if(_horizontal_polarity < 0) {
 242     _head_belly_vx =   _head_belly_uy;
 243     _head_belly_vy = - _head_belly_ux;
 244   } else {
 245     _head_belly_vx = - _head_belly_uy;
 246     _head_belly_vy =   _head_belly_ux;
 247   }
 248
 249   scalar_t l = sqrt(_head_belly_vx * _head_belly_vx + _head_belly_vy * _head_belly_vy);
 250
 251   _head_belly_vx = (_head_belly_vx / l) * _head_radius * 2;
 252   _head_belly_vy = (_head_belly_vy / l) * _head_radius * 2;
 253   _head_belly_edge_shift = int(floor(- RichImage::nb_edge_tags * atan2(_head_belly_ux, _head_belly_uy) / (2 * M_PI) + 0.5));
 254   _head_belly_edge_shift = (RichImage::nb_edge_tags + _head_belly_edge_shift) % RichImage::nb_edge_tags;
 255 }
 256
 257 int PiReferential::common_scale() {
 258   return _common_scale;
 259 }
 260
 261 void PiReferential::register_rectangle(int registration_mode,
 262                                        Rectangle *original,
 263                                        Rectangle *result) {
 264   scalar_t alpha, beta , xc, yc, w, h;
 265
 266   alpha = (original->xmin + original->xmax) * 0.5;
 267   beta  = (original->ymin + original->ymax) * 0.5;
 268
 269   switch(registration_mode) {
 270
 271   case RM_HEAD:
 272     {
 273       xc = _head_xc + alpha * _head_ux + beta * _head_vx;
 274       yc = _head_yc + alpha * _head_uy + beta * _head_vy;
 275       w = (original->xmax - original->xmin) * _head_window_scaling;
 276       h = (original->ymax - original->ymin) * _head_window_scaling;
 277     }
 278     break;
 279
 280   case RM_HEAD_NO_POLARITY:
 281     {
 282       xc = _head_xc + alpha * _head_ux_nopolarity + beta * _head_vx_nopolarity;
 283       yc = _head_yc + alpha * _head_uy_nopolarity + beta * _head_vy_nopolarity;
 284       w = (original->xmax - original->xmin) * _head_window_scaling;
 285       h = (original->ymax - original->ymin) * _head_window_scaling;
 286     }
 287     break;
 288
 289   case RM_BELLY:
 290     {
 291       xc = _belly_xc + alpha * _belly_ux + beta * _belly_vx;
 292       yc = _belly_yc + alpha * _belly_uy + beta * _belly_vy;
 293       w = (original->xmax - original->xmin) * _belly_window_scaling;
 294       h = (original->ymax - original->ymin) * _belly_window_scaling;
 295     }
 296     break;
 297
 298   case RM_BELLY_NO_POLARITY:
 299     {
 300       xc = _belly_xc + alpha * _belly_ux_nopolarity + beta * _belly_vx_nopolarity;
 301       yc = _belly_yc + alpha * _belly_uy_nopolarity + beta * _belly_vy_nopolarity;
 302       w = (original->xmax - original->xmin) * _belly_window_scaling;
 303       h = (original->ymax - original->ymin) * _belly_window_scaling;
 304     }
 305     break;
 306
 307   case RM_HEAD_BELLY:
 308   case RM_HEAD_BELLY_EDGES:
 309     {
 310       xc = _head_belly_xc + alpha * _head_belly_ux + beta * _head_belly_vx;
 311       yc = _head_belly_yc + alpha * _head_belly_uy + beta * _head_belly_vy;
 312       w = (original->xmax - original->xmin) * _head_window_scaling;
 313       h = (original->ymax - original->ymin) * _head_window_scaling;
 314     }
 315     break;
 316
 317   default:
 318     cerr << "Undefined registration mode." << endl;
 319     abort();
 320   }
 321
 322   result->xmin = xc - 0.5 * w;
 323   result->ymin = yc - 0.5 * h;
 324   result->xmax = xc + 0.5 * w;
 325   result->ymax = yc + 0.5 * h;
 326
 327   ASSERT(result->xmin < result->xmax && result->ymin < result->ymax);
 328 }
 329
 330 int PiReferential::register_edge(int registration_mode, int edge_type) {
 331
 332   if(edge_type >= RichImage::first_edge_tag &&
 333      edge_type < RichImage::first_edge_tag + RichImage::nb_edge_tags) {
 334
 335     int e = edge_type - RichImage::first_edge_tag;
 336
 337     switch(registration_mode) {
 338     case PiReferential::RM_HEAD_NO_POLARITY:
 339     case PiReferential::RM_BELLY_NO_POLARITY:
 340       break;
 341
 342     case PiReferential::RM_HEAD:
 343     case PiReferential::RM_BELLY:
 344     case PiReferential::RM_HEAD_BELLY:
 345       if(_horizontal_polarity < 0) {
 346         e = (RichImage::nb_edge_tags - e) % RichImage::nb_edge_tags;
 347       }
 348       break;
 349
 350     case PiReferential::RM_HEAD_BELLY_EDGES:
 351       if(_horizontal_polarity < 0) {
 352         e = (RichImage::nb_edge_tags - e) % RichImage::nb_edge_tags;
 353       }
 354       e += _head_belly_edge_shift;
 355       break;
 356
 357     default:
 358       cerr << "INCONSISTENCY" << endl;
 359       abort();
 360     }
 361
 362     e = e % RichImage::nb_edge_tags;
 363
 364     return RichImage::first_edge_tag + e;
 365
 366   }
 367
 368   else return edge_type;
 369 }
 370
 371 void PiReferential::draw(RGBImage *image, int level) {
 372   int x1, y1, x2, y2, x3, y3, x4, y4;
 373
 374   if(level >= 1) {
 375
 376     // Draw the RM_BELLY reference frame
 377
 378     x1 = int(_belly_xc + _belly_ux + _belly_vx);
 379     y1 = int(_belly_yc + _belly_uy + _belly_vy);
 380     x2 = int(_belly_xc - _belly_ux + _belly_vx);
 381     y2 = int(_belly_yc - _belly_uy + _belly_vy);
 382     x3 = int(_belly_xc - _belly_ux - _belly_vx);
 383     y3 = int(_belly_yc - _belly_uy - _belly_vy);
 384     x4 = int(_belly_xc + _belly_ux - _belly_vx);
 385     y4 = int(_belly_yc + _belly_uy - _belly_vy);
 386
 387     draw_frame(image, RM_BELLY, x1, y1, x2, y2, x3, y3, x4, y4);
 388
 389     // Draw the RM_HEAD_BELLY reference frame
 390
 391     x1 = int(_head_belly_xc + _head_belly_ux + _head_belly_vx);
 392     y1 = int(_head_belly_yc + _head_belly_uy + _head_belly_vy);
 393     x2 = int(_head_belly_xc - _head_belly_ux + _head_belly_vx);
 394     y2 = int(_head_belly_yc - _head_belly_uy + _head_belly_vy);
 395     x3 = int(_head_belly_xc - _head_belly_ux - _head_belly_vx);
 396     y3 = int(_head_belly_yc - _head_belly_uy - _head_belly_vy);
 397     x4 = int(_head_belly_xc + _head_belly_ux - _head_belly_vx);
 398     y4 = int(_head_belly_yc + _head_belly_uy - _head_belly_vy);
 399
 400     draw_frame(image, RM_HEAD_BELLY, x1, y1, x2, y2, x3, y3, x4, y4);
 401   }
 402
 403   // Draw the RM_HEAD reference frame
 404
 405   x1 = int(_head_xc + _head_ux + _head_vx);
 406   y1 = int(_head_yc + _head_uy + _head_vy);
 407   x2 = int(_head_xc - _head_ux + _head_vx);
 408   y2 = int(_head_yc - _head_uy + _head_vy);
 409   x3 = int(_head_xc - _head_ux - _head_vx);
 410   y3 = int(_head_yc - _head_uy - _head_vy);
 411   x4 = int(_head_xc + _head_ux - _head_vx);
 412   y4 = int(_head_yc + _head_uy - _head_vy);
 413
 414   draw_frame(image, RM_HEAD, x1, y1, x2, y2, x3, y3, x4, y4);
 415 }
 416
 417 void PiReferential::print_registration_mode(ostream *out, int registration_mode) {
 418   switch(registration_mode) {
 419   case RM_HEAD:
 420     (*out) << "RM_HEAD";
 421     break;
 422   case RM_HEAD_NO_POLARITY:
 423     (*out) << "RM_HEAD_NO_POLARITY";
 424     break;
 425   case RM_BELLY:
 426     (*out) << "RM_BELLY";
 427     break;
 428   case RM_BELLY_NO_POLARITY:
 429     (*out) << "RM_BELLY_NO_POLARITY";
 430     break;
 431   case RM_HEAD_BELLY:
 432     (*out) << "RM_HEAD_BELLY";
 433     break;
 434   case RM_HEAD_BELLY_EDGES:
 435     (*out) << "RM_HEAD_BELLY_EDGES";
 436     break;
 437   default:
 438     abort();
 439   }
 440 }