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