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                                           //
  16 // (C) Idiap Research Institute                                          //
  17 //                                                                       //
  18 // Contact <francois.fleuret@idiap.ch> for comments & bug reports        //
  19 ///////////////////////////////////////////////////////////////////////////
  20
  21 #include "pi_referential.h"
  22 #include "global.h"
  23 #include "rich_image.h"
  24
  25 void PiReferential::draw_frame(RGBImage *image,
  26                                int registration_mode,
  27                                int x1, int y1,
  28                                int x2, int y2,
  29                                int x3, int y3,
  30                                int x4, int y4) {
  31
  32   int r, g, b;
  33
  34   switch(registration_mode) {
  35
  36   case PiReferential::RM_HEAD:
  37     r = 0; g = 255; b = 0;
  38     break;
  39
  40   case PiReferential::RM_HEAD_NO_POLARITY:
  41     r = 128; g = 255; b = 128;
  42     break;
  43
  44   case PiReferential::RM_BELLY:
  45     r = 64; g = 0; b = 255;
  46     break;
  47
  48   case PiReferential::RM_BELLY_NO_POLARITY:
  49     r = 192; g = 128; b = 255;
  50     break;
  51
  52   case PiReferential::RM_HEAD_BELLY:
  53   case PiReferential::RM_HEAD_BELLY_EDGES:
  54     r = 255; g = 0; b = 0;
  55     break;
  56
  57   default:
  58     cerr << "INCONSISTENCY" << endl;
  59     abort();
  60   }
  61
  62   if(global.pictures_for_article) {
  63     r = 255; g = 255; b = 255;
  64     image->draw_line(6, r, g, b, x1, y1, x2, y2);
  65     image->draw_line(6, r, g, b, x2, y2, x3, y3);
  66     image->draw_line(6, r, g, b, x3, y3, x4, y4);
  67     image->draw_line(6, r, g, b, x4, y4, x1, y1);
  68
  69     r =   0; g =   0; b =   0;
  70     image->draw_line(2, r, g, b, x1, y1, x2, y2);
  71     image->draw_line(2, r, g, b, x2, y2, x3, y3);
  72     image->draw_line(2, r, g, b, x3, y3, x4, y4);
  73     image->draw_line(2, r, g, b, x4, y4, x1, y1);
  74   } else {
  75     image->draw_line(2, r, g, b, x1, y1, x2, y2);
  76     image->draw_line(2, r, g, b, x2, y2, x3, y3);
  77     image->draw_line(2, r, g, b, x3, y3, x4, y4);
  78     image->draw_line(2, r, g, b, x4, y4, x1, y1);
  79   }
  80 }
  81
  82 void PiReferential::draw_window(RGBImage *image,
  83                                 int registration_mode, Rectangle *window,
  84                                 int filled) {
  85   int r, g, b;
  86
  87   switch(registration_mode) {
  88
  89   case PiReferential::RM_HEAD:
  90     r = 0; g = 255; b = 0;
  91     break;
  92
  93   case PiReferential::RM_HEAD_NO_POLARITY:
  94     r = 128; g = 255; b = 128;
  95     break;
  96
  97   case PiReferential::RM_BELLY:
  98     r = 64; g = 0; b = 255;
  99     break;
 100
 101   case PiReferential::RM_BELLY_NO_POLARITY:
 102     r = 192; g = 128; b = 255;
 103     break;
 104
 105   case PiReferential::RM_HEAD_BELLY:
 106   case PiReferential::RM_HEAD_BELLY_EDGES:
 107     r = 255; g = 0; b = 0;
 108     break;
 109
 110   default:
 111     cerr << "INCONSISTENCY" << endl;
 112     abort();
 113   }
 114
 115   int xmin = int(window->xmin);
 116   int ymin = int(window->ymin);
 117   int xmax = int(window->xmax);
 118   int ymax = int(window->ymax);
 119
 120   if(global.pictures_for_article) {
 121     r = 255; g = 255; b = 255;
 122     image->draw_line(6, r, g, b, xmin, ymin, xmax, ymin);
 123     image->draw_line(6, r, g, b, xmax, ymin, xmax, ymax);
 124     image->draw_line(6, r, g, b, xmax, ymax, xmin, ymax);
 125     image->draw_line(6, r, g, b, xmin, ymax, xmin, ymin);
 126
 127     r =   0; g =   0; b =   0;
 128     image->draw_line(2, r, g, b, xmin, ymin, xmax, ymin);
 129     image->draw_line(2, r, g, b, xmax, ymin, xmax, ymax);
 130     image->draw_line(2, r, g, b, xmax, ymax, xmin, ymax);
 131     image->draw_line(2, r, g, b, xmin, ymax, xmin, ymin);
 132
 133   } else {
 134     image->draw_line(2, r, g, b, xmin, ymin, xmax, ymin);
 135     image->draw_line(2, r, g, b, xmax, ymin, xmax, ymax);
 136     image->draw_line(2, r, g, b, xmax, ymax, xmin, ymax);
 137     image->draw_line(2, r, g, b, xmin, ymax, xmin, ymin);
 138     if(filled) {
 139       int delta = 4;
 140       for(int d = ymin - ymax; d <= xmax - xmin; d += delta) {
 141         int x1 = xmin + d;
 142         int y1 = ymin;
 143         int x2 = xmin + d + ymax - ymin;
 144         int y2 = ymax;
 145         if(x1 < xmin) { y1 = y1 + (xmin - x1); x1 = xmin; }
 146         if(x2 > xmax) { y2 = y2 - (x2 - xmax); x2 = xmax; }
 147         image->draw_line(1, r, g, b, x1, y1, x2, y2);
 148       }
 149     }
 150   }
 151
 152 }
 153
 154 PiReferential::PiReferential(PoseCell *cell) {
 155   scalar_t head_radius = sqrt(scalar_t(cell->_head_radius.min * cell->_head_radius.max));
 156
 157   _common_scale = global.scale_to_discrete_log_scale(head_radius / global.min_head_radius);
 158
 159   scalar_t discrete_scale_ratio = global.discrete_log_scale_to_scale(_common_scale);
 160
 161   //////////////////////////////////////////////////////////////////////
 162   // Locations and scales
 163
 164   // Head location
 165
 166   _head_xc = cell->_head_xc.middle() * discrete_scale_ratio;
 167   _head_yc = cell->_head_yc.middle() * discrete_scale_ratio;
 168   _head_radius = cell->_head_radius.middle() * discrete_scale_ratio;
 169   _head_window_scaling = _head_radius * 2.0;
 170
 171   // Body location
 172
 173   // **********************************************************************
 174   // Useless code, but necessary to keep the exact same results with
 175   // g++ 4.1 and -O3 options on reference experiments.
 176   _body_xc = cell->_belly_xc.middle() * discrete_scale_ratio;
 177   _body_yc = cell->_belly_yc.middle() * discrete_scale_ratio;
 178   _body_tilt = 0;
 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
 184   // Belly location
 185
 186   const scalar_t belly_frame_factor = 2.0;
 187
 188   _belly_xc = cell->_belly_xc.middle() * discrete_scale_ratio;
 189   _belly_yc = cell->_belly_yc.middle() * discrete_scale_ratio;
 190   _belly_window_scaling = _head_window_scaling * belly_frame_factor;
 191
 192   // Head-belly location
 193
 194   _head_belly_xc = (_head_xc + _belly_xc) * 0.5;
 195   _head_belly_yc = (_head_yc + _belly_yc) * 0.5;
 196
 197   //////////////////////////////////////////////////////////////////////
 198   // Frames
 199
 200   if(_body_xc >= _head_xc) {
 201     //   if(_belly_xc >= _head_xc) {
 202     _horizontal_polarity = 1;
 203   } else {
 204     _horizontal_polarity = -1;
 205   }
 206
 207   // Head frame
 208
 209   if(_horizontal_polarity < 0) {
 210     _head_ux = _head_radius * 2.0;
 211     _head_uy = 0;
 212   } else {
 213     _head_ux = - _head_radius * 2.0;
 214     _head_uy = 0;
 215   }
 216
 217   _head_vx = 0;
 218   _head_vy = - _head_radius * 2.0;
 219
 220   _head_ux_nopolarity = _head_radius * 2.0;
 221   _head_uy_nopolarity = 0;
 222   _head_vx_nopolarity = 0;
 223   _head_vy_nopolarity = - _head_radius * 2.0;
 224
 225   // Belly frame
 226
 227   _belly_ux = _head_ux * belly_frame_factor;
 228   _belly_uy = _head_uy * belly_frame_factor;
 229   _belly_vx = _head_vx * belly_frame_factor;
 230   _belly_vy = _head_vy * belly_frame_factor;
 231
 232   _belly_ux_nopolarity = _head_ux_nopolarity * belly_frame_factor;
 233   _belly_uy_nopolarity = _head_uy_nopolarity * belly_frame_factor;
 234   _belly_vx_nopolarity = _head_vx_nopolarity * belly_frame_factor;
 235   _belly_vy_nopolarity = _head_vy_nopolarity * belly_frame_factor;
 236
 237   // Head-belly frame
 238
 239   _head_belly_ux = 2 * (_head_xc - _head_belly_xc);
 240   _head_belly_uy = 2 * (_head_yc - _head_belly_yc);
 241
 242   if(_horizontal_polarity < 0) {
 243     _head_belly_vx =   _head_belly_uy;
 244     _head_belly_vy = - _head_belly_ux;
 245   } else {
 246     _head_belly_vx = - _head_belly_uy;
 247     _head_belly_vy =   _head_belly_ux;
 248   }
 249
 250   scalar_t l = sqrt(_head_belly_vx * _head_belly_vx + _head_belly_vy * _head_belly_vy);
 251
 252   _head_belly_vx = (_head_belly_vx / l) * _head_radius * 2;
 253   _head_belly_vy = (_head_belly_vy / l) * _head_radius * 2;
 254   _head_belly_edge_shift = int(floor(- RichImage::nb_edge_tags * atan2(_head_belly_ux, _head_belly_uy) / (2 * M_PI) + 0.5));
 255   _head_belly_edge_shift = (RichImage::nb_edge_tags + _head_belly_edge_shift) % RichImage::nb_edge_tags;
 256 }
 257
 258 int PiReferential::common_scale() {
 259   return _common_scale;
 260 }
 261
 262 void PiReferential::register_rectangle(int registration_mode,
 263                                        Rectangle *original,
 264                                        Rectangle *result) {
 265   scalar_t alpha, beta , xc, yc, w, h;
 266
 267   alpha = (original->xmin + original->xmax) * 0.5;
 268   beta  = (original->ymin + original->ymax) * 0.5;
 269
 270   switch(registration_mode) {
 271
 272   case RM_HEAD:
 273     {
 274       xc = _head_xc + alpha * _head_ux + beta * _head_vx;
 275       yc = _head_yc + alpha * _head_uy + beta * _head_vy;
 276       w = (original->xmax - original->xmin) * _head_window_scaling;
 277       h = (original->ymax - original->ymin) * _head_window_scaling;
 278     }
 279     break;
 280
 281   case RM_HEAD_NO_POLARITY:
 282     {
 283       xc = _head_xc + alpha * _head_ux_nopolarity + beta * _head_vx_nopolarity;
 284       yc = _head_yc + alpha * _head_uy_nopolarity + beta * _head_vy_nopolarity;
 285       w = (original->xmax - original->xmin) * _head_window_scaling;
 286       h = (original->ymax - original->ymin) * _head_window_scaling;
 287     }
 288     break;
 289
 290   case RM_BELLY:
 291     {
 292       xc = _belly_xc + alpha * _belly_ux + beta * _belly_vx;
 293       yc = _belly_yc + alpha * _belly_uy + beta * _belly_vy;
 294       w = (original->xmax - original->xmin) * _belly_window_scaling;
 295       h = (original->ymax - original->ymin) * _belly_window_scaling;
 296     }
 297     break;
 298
 299   case RM_BELLY_NO_POLARITY:
 300     {
 301       xc = _belly_xc + alpha * _belly_ux_nopolarity + beta * _belly_vx_nopolarity;
 302       yc = _belly_yc + alpha * _belly_uy_nopolarity + beta * _belly_vy_nopolarity;
 303       w = (original->xmax - original->xmin) * _belly_window_scaling;
 304       h = (original->ymax - original->ymin) * _belly_window_scaling;
 305     }
 306     break;
 307
 308   case RM_HEAD_BELLY:
 309   case RM_HEAD_BELLY_EDGES:
 310     {
 311       xc = _head_belly_xc + alpha * _head_belly_ux + beta * _head_belly_vx;
 312       yc = _head_belly_yc + alpha * _head_belly_uy + beta * _head_belly_vy;
 313       w = (original->xmax - original->xmin) * _head_window_scaling;
 314       h = (original->ymax - original->ymin) * _head_window_scaling;
 315     }
 316     break;
 317
 318   default:
 319     cerr << "Undefined registration mode." << endl;
 320     abort();
 321   }
 322
 323   result->xmin = xc - 0.5 * w;
 324   result->ymin = yc - 0.5 * h;
 325   result->xmax = xc + 0.5 * w;
 326   result->ymax = yc + 0.5 * h;
 327
 328   ASSERT(result->xmin < result->xmax && result->ymin < result->ymax);
 329 }
 330
 331 int PiReferential::register_edge(int registration_mode, int edge_type) {
 332
 333   if(edge_type >= RichImage::first_edge_tag &&
 334      edge_type < RichImage::first_edge_tag + RichImage::nb_edge_tags) {
 335
 336     int e = edge_type - RichImage::first_edge_tag;
 337
 338     switch(registration_mode) {
 339     case PiReferential::RM_HEAD_NO_POLARITY:
 340     case PiReferential::RM_BELLY_NO_POLARITY:
 341       break;
 342
 343     case PiReferential::RM_HEAD:
 344     case PiReferential::RM_BELLY:
 345     case PiReferential::RM_HEAD_BELLY:
 346       if(_horizontal_polarity < 0) {
 347         e = (RichImage::nb_edge_tags - e) % RichImage::nb_edge_tags;
 348       }
 349       break;
 350
 351     case PiReferential::RM_HEAD_BELLY_EDGES:
 352       if(_horizontal_polarity < 0) {
 353         e = (RichImage::nb_edge_tags - e) % RichImage::nb_edge_tags;
 354       }
 355       e += _head_belly_edge_shift;
 356       break;
 357
 358     default:
 359       cerr << "INCONSISTENCY" << endl;
 360       abort();
 361     }
 362
 363     e = e % RichImage::nb_edge_tags;
 364
 365     return RichImage::first_edge_tag + e;
 366
 367   }
 368
 369   else return edge_type;
 370 }
 371
 372 void PiReferential::draw(RGBImage *image, int level) {
 373   int x1, y1, x2, y2, x3, y3, x4, y4;
 374
 375   if(level >= 1) {
 376
 377     // Draw the RM_BELLY reference frame
 378
 379     x1 = int(_belly_xc + _belly_ux + _belly_vx);
 380     y1 = int(_belly_yc + _belly_uy + _belly_vy);
 381     x2 = int(_belly_xc - _belly_ux + _belly_vx);
 382     y2 = int(_belly_yc - _belly_uy + _belly_vy);
 383     x3 = int(_belly_xc - _belly_ux - _belly_vx);
 384     y3 = int(_belly_yc - _belly_uy - _belly_vy);
 385     x4 = int(_belly_xc + _belly_ux - _belly_vx);
 386     y4 = int(_belly_yc + _belly_uy - _belly_vy);
 387
 388     draw_frame(image, RM_BELLY, x1, y1, x2, y2, x3, y3, x4, y4);
 389
 390     // Draw the RM_HEAD_BELLY reference frame
 391
 392     x1 = int(_head_belly_xc + _head_belly_ux + _head_belly_vx);
 393     y1 = int(_head_belly_yc + _head_belly_uy + _head_belly_vy);
 394     x2 = int(_head_belly_xc - _head_belly_ux + _head_belly_vx);
 395     y2 = int(_head_belly_yc - _head_belly_uy + _head_belly_vy);
 396     x3 = int(_head_belly_xc - _head_belly_ux - _head_belly_vx);
 397     y3 = int(_head_belly_yc - _head_belly_uy - _head_belly_vy);
 398     x4 = int(_head_belly_xc + _head_belly_ux - _head_belly_vx);
 399     y4 = int(_head_belly_yc + _head_belly_uy - _head_belly_vy);
 400
 401     draw_frame(image, RM_HEAD_BELLY, x1, y1, x2, y2, x3, y3, x4, y4);
 402   }
 403
 404   // Draw the RM_HEAD reference frame
 405
 406   x1 = int(_head_xc + _head_ux + _head_vx);
 407   y1 = int(_head_yc + _head_uy + _head_vy);
 408   x2 = int(_head_xc - _head_ux + _head_vx);
 409   y2 = int(_head_yc - _head_uy + _head_vy);
 410   x3 = int(_head_xc - _head_ux - _head_vx);
 411   y3 = int(_head_yc - _head_uy - _head_vy);
 412   x4 = int(_head_xc + _head_ux - _head_vx);
 413   y4 = int(_head_yc + _head_uy - _head_vy);
 414
 415   draw_frame(image, RM_HEAD, x1, y1, x2, y2, x3, y3, x4, y4);
 416 }
 417
 418 void PiReferential::print_registration_mode(ostream *out, int registration_mode) {
 419   switch(registration_mode) {
 420   case RM_HEAD:
 421     (*out) << "RM_HEAD";
 422     break;
 423   case RM_HEAD_NO_POLARITY:
 424     (*out) << "RM_HEAD_NO_POLARITY";
 425     break;
 426   case RM_BELLY:
 427     (*out) << "RM_BELLY";
 428     break;
 429   case RM_BELLY_NO_POLARITY:
 430     (*out) << "RM_BELLY_NO_POLARITY";
 431     break;
 432   case RM_HEAD_BELLY:
 433     (*out) << "RM_HEAD_BELLY";
 434     break;
 435   case RM_HEAD_BELLY_EDGES:
 436     (*out) << "RM_HEAD_BELLY_EDGES";
 437     break;
 438   default:
 439     abort();
 440   }
 441 }