pose_cell_hierarchy.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 "pose_cell_hierarchy.h"
  20 #include "gaussian.h"
  21
  22 PoseCellHierarchy::PoseCellHierarchy() {
  23   _body_cells = 0;
  24 }
  25
  26 PoseCellHierarchy::PoseCellHierarchy(LabelledImagePool *train_pool) {
  27   _nb_levels = global.nb_levels;
  28   _min_head_radius = global.min_head_radius;
  29   _max_head_radius = global.max_head_radius;
  30   _root_cell_nb_xy_per_scale = global.root_cell_nb_xy_per_scale;
  31
  32   LabelledImage *image;
  33   int nb_total_targets = 0;
  34   for(int i = 0; i < train_pool->nb_images(); i++) {
  35     image = train_pool->grab_image(i);
  36     // We are going to symmetrize
  37     nb_total_targets += 2 * image->nb_targets();
  38     train_pool->release_image(i);
  39   }
  40
  41   RelativeBodyPoseCell targets[nb_total_targets];
  42
  43   int u = 0;
  44   for(int i = 0; i < train_pool->nb_images(); i++) {
  45     image = train_pool->grab_image(i);
  46
  47     PoseCellSet cell_set;
  48     add_root_cells(image, &cell_set);
  49
  50     for(int t = 0; t < image->nb_targets(); t++) {
  51       Pose pose = *image->get_target_pose(t);
  52       Pose coarse;
  53
  54       cell_set.get_containing_cell(&pose)->get_centroid(&coarse);
  55
  56       targets[u]._body_xc.set((pose._body_xc - coarse._head_xc) / coarse._head_radius);
  57       targets[u]._body_yc.set((pose._body_yc - coarse._head_yc) / coarse._head_radius);
  58       u++;
  59
  60       pose.horizontal_flip(image->width());
  61
  62       cell_set.get_containing_cell(&pose)->get_centroid(&coarse);
  63
  64       targets[u]._body_xc.set((pose._body_xc - coarse._head_xc) / coarse._head_radius);
  65       targets[u]._body_yc.set((pose._body_yc - coarse._head_yc) / coarse._head_radius);
  66       u++;
  67     }
  68
  69     train_pool->release_image(i);
  70   }
  71
  72   scalar_t fattening = 1.1;
  73
  74   Interval body_rxc, body_ryc;
  75
  76   body_rxc.set(&targets[0]._body_xc);
  77   body_ryc.set(&targets[0]._body_yc);
  78
  79   for(int t = 0; t < nb_total_targets; t++) {
  80     body_rxc.swallow(&targets[t]._body_xc);
  81     body_ryc.swallow(&targets[t]._body_yc);
  82   }
  83
  84   body_rxc.min *= fattening;
  85   body_rxc.max *= fattening;
  86   body_ryc.min *= fattening;
  87   body_ryc.max *= fattening;
  88
  89   scalar_t body_rxc_min = body_resolution * floor(body_rxc.min / body_resolution);
  90   int nb_body_rxc = int(ceil((body_rxc.max - body_rxc_min) / body_resolution));
  91
  92   scalar_t body_ryc_min = body_resolution * floor(body_ryc.min / body_resolution);
  93   int nb_body_ryc = int(ceil((body_ryc.max - body_ryc_min) / body_resolution));
  94
  95   (*global.log_stream) << "body_rxc = " << body_rxc << endl
  96                        << "body_rxc_min = " << body_rxc_min << endl
  97                        << "body_rxc_min + nb_body_rxc * body_resolution = " << body_rxc_min + nb_body_rxc * body_resolution << endl
  98                        << endl
  99                        << "body_ryc = " << body_ryc << endl
 100                        << "body_ryc_min = " << body_ryc_min << endl
 101                        << "body_ryc_min + nb_body_ryc * body_resolution = " << body_ryc_min + nb_body_ryc * body_resolution << endl;
 102
 103   int used[nb_body_rxc * nb_body_rxc];
 104
 105   for(int k = 0; k < nb_body_rxc * nb_body_ryc; k++) {
 106     used[k] = 1;
 107   }
 108
 109   // An ugly way to compute the convexe enveloppe
 110
 111   for(scalar_t alpha = 0; alpha < M_PI * 2; alpha += (2 * M_PI) / 100) {
 112     scalar_t vx = cos(alpha), vy = sin(alpha);
 113     scalar_t rho = 0;
 114
 115     for(int t = 0; t < nb_total_targets; t++) {
 116       rho = min(rho, vx * targets[t]._body_xc.middle() + vy * targets[t]._body_yc.middle());
 117     }
 118
 119     rho *= fattening;
 120
 121     for(int j = 0; j < nb_body_ryc; j++) {
 122       for(int i = 0; i < nb_body_rxc; i++) {
 123         if(
 124            vx * (scalar_t(i + 0) * body_resolution + body_rxc_min) +
 125            vy * (scalar_t(j + 0) * body_resolution + body_ryc_min) < rho
 126            &&
 127            vx * (scalar_t(i + 1) * body_resolution + body_rxc_min) +
 128            vy * (scalar_t(j + 0) * body_resolution + body_ryc_min) < rho
 129            &&
 130            vx * (scalar_t(i + 0) * body_resolution + body_rxc_min) +
 131            vy * (scalar_t(j + 1) * body_resolution + body_ryc_min) < rho
 132            &&
 133            vx * (scalar_t(i + 1) * body_resolution + body_rxc_min) +
 134            vy * (scalar_t(j + 1) * body_resolution + body_ryc_min) < rho
 135            ) {
 136           used[i + j * nb_body_rxc] = 0;
 137         }
 138       }
 139     }
 140   }
 141
 142   _nb_body_cells = 0;
 143   for(int j = 0; j < nb_body_ryc; j++) {
 144     for(int i = 0; i < nb_body_rxc; i++) {
 145       if(used[i + nb_body_rxc * j]) {
 146         _nb_body_cells++;
 147       }
 148     }
 149   }
 150
 151   _body_cells = new RelativeBodyPoseCell[_nb_body_cells];
 152
 153   for(int j = 0; j < nb_body_ryc; j++) {
 154     for(int i = 0; i < nb_body_rxc; i++) {
 155       if(used[i + nb_body_rxc * j]) {
 156         if(sq(scalar_t(i) * body_resolution + body_resolution/2 + body_rxc_min) +
 157            sq(scalar_t(j) * body_resolution + body_resolution/2 + body_ryc_min) <= 1) {
 158           (*global.log_stream) << "*";
 159         } else {
 160           (*global.log_stream) << "X";
 161         }
 162       } else {
 163         (*global.log_stream) << ".";
 164       }
 165     }
 166     (*global.log_stream) << endl;
 167   }
 168
 169   int k = 0;
 170   for(int j = 0; j < nb_body_ryc; j++) {
 171     for(int i = 0; i < nb_body_rxc; i++) {
 172
 173       if(used[i + nb_body_rxc * j]) {
 174
 175         RelativeBodyPoseCell mother;
 176
 177         scalar_t x = scalar_t(i) * body_resolution + body_rxc_min;
 178         scalar_t y = scalar_t(j) * body_resolution + body_ryc_min;
 179
 180         mother._body_xc.set(x, x + body_resolution);
 181         mother._body_yc.set(y, y + body_resolution);
 182
 183         //         scalar_t dist_min = body_resolution;
 184         scalar_t dist_min = 1e6;
 185
 186         int nb_got;
 187
 188         Gaussian dist_body_radius_1, dist_body_radius_2, dist_body_tilt;
 189
 190         do {
 191
 192           nb_got = 0;
 193
 194           for(int t = 0; t < nb_total_targets; t++) {
 195
 196             scalar_t dist =
 197               sqrt(sq(targets[t]._body_xc.middle() - x - body_resolution / 2) +
 198                    sq(targets[t]._body_yc.middle() - y - body_resolution / 2));
 199
 200             if(dist <= dist_min) {
 201               dist_body_radius_1.add_sample(targets[t]._body_radius_1.middle());
 202               dist_body_radius_2.add_sample(targets[t]._body_radius_2.middle());
 203               dist_body_tilt.add_sample(targets[t]._body_tilt.middle());
 204               nb_got++;
 205             }
 206
 207           }
 208
 209           dist_min *= 2.0;
 210         } while(nb_got < min(100, nb_total_targets));
 211
 212         scalar_t zeta = 4;
 213
 214         mother._body_radius_1.set(dist_body_radius_1.expectation() -
 215                                   zeta * dist_body_radius_1.standard_deviation(),
 216                                   dist_body_radius_1.expectation() +
 217                                   zeta * dist_body_radius_1.standard_deviation());
 218
 219         mother._body_radius_2.set(dist_body_radius_2.expectation() -
 220                                   zeta * dist_body_radius_2.standard_deviation(),
 221                                   dist_body_radius_2.expectation() +
 222                                   zeta * dist_body_radius_2.standard_deviation());
 223
 224         mother._body_tilt.set(dist_body_tilt.expectation() -
 225                               zeta * dist_body_tilt.standard_deviation(),
 226                               dist_body_tilt.expectation() +
 227                               zeta * dist_body_tilt.standard_deviation());
 228
 229         _body_cells[k++] = mother;
 230       }
 231     }
 232   }
 233
 234   (*global.log_stream) << _nb_body_cells << " body cells." << endl;
 235 }
 236
 237 PoseCellHierarchy::~PoseCellHierarchy() {
 238   delete[] _body_cells;
 239 }
 240
 241 int PoseCellHierarchy::nb_levels() {
 242   return _nb_levels;
 243 }
 244
 245 void PoseCellHierarchy::get_containing_cell(Image *image, int level,
 246                                             Pose *pose, PoseCell *result_cell) {
 247   PoseCellSet cell_set;
 248
 249   for(int l = 0; l < level + 1; l++) {
 250     cell_set.erase_content();
 251     if(l == 0) {
 252       add_root_cells(image, &cell_set);
 253     } else {
 254       add_subcells(l, result_cell, &cell_set);
 255     }
 256
 257     *result_cell = *(cell_set.get_containing_cell(pose));
 258   }
 259 }
 260
 261 void PoseCellHierarchy::add_root_cells(Image *image, PoseCellSet *cell_set) {
 262
 263   const int nb_scales = int((log(_max_head_radius) - log(_min_head_radius)) / log(2) *
 264                             global.nb_scales_per_power_of_two);
 265
 266   scalar_t alpha = log(_min_head_radius);
 267   scalar_t beta = log(2) / scalar_t(global.nb_scales_per_power_of_two);
 268
 269   for(int s = 0; s < nb_scales; s++) {
 270     scalar_t cell_xy_size = exp(alpha + scalar_t(s) * beta) / global.root_cell_nb_xy_per_scale;
 271     PoseCell cell;
 272     cell._head_radius.min = exp(alpha + scalar_t(s) * beta);
 273     cell._head_radius.max = exp(alpha + scalar_t(s+1) * beta);
 274     cell._head_tilt.min = -M_PI;
 275     cell._head_tilt.max = M_PI;
 276     for(scalar_t y = 0; y < image->height(); y += cell_xy_size)
 277       for(scalar_t x = 0; x < image->width(); x += cell_xy_size) {
 278         cell._head_xc.min = x;
 279         cell._head_xc.max = x + cell_xy_size;
 280         cell._head_yc.min = y;
 281         cell._head_yc.max = y + cell_xy_size;
 282         cell._body_xc.min = cell._head_xc.min - pseudo_infty;
 283         cell._body_xc.max = cell._head_xc.max + pseudo_infty;
 284         cell._body_yc.min = cell._head_yc.min - pseudo_infty;
 285         cell._body_yc.max = cell._head_yc.max + pseudo_infty;
 286         cell_set->add_cell(&cell);
 287       }
 288   }
 289 }
 290
 291 void PoseCellHierarchy::add_subcells(int level, PoseCell *root,
 292                                      PoseCellSet *cell_set) {
 293
 294   switch(level) {
 295
 296   case 1:
 297     {
 298       // Here we split the body-center coordinate cell part
 299       PoseCell cell = *root;
 300       scalar_t r = sqrt(cell._head_radius.min * cell._head_radius.max);
 301       scalar_t x = (cell._head_xc.min + cell._head_xc.max) / 2.0;
 302       scalar_t y = (cell._head_yc.min + cell._head_yc.max) / 2.0;
 303       for(int k = 0; k < _nb_body_cells; k++) {
 304         cell._body_xc.min = (_body_cells[k]._body_xc.min * r) + x;
 305         cell._body_xc.max = (_body_cells[k]._body_xc.max * r) + x;
 306         cell._body_yc.min = (_body_cells[k]._body_yc.min * r) + y;
 307         cell._body_yc.max = (_body_cells[k]._body_yc.max * r) + y;
 308         cell_set->add_cell(&cell);
 309       }
 310     }
 311     break;
 312
 313   default:
 314     {
 315       cerr << "Inconsistent level in PoseCellHierarchy::add_subcells" << endl;
 316       abort();
 317     }
 318     break;
 319   }
 320 }
 321
 322
 323 int PoseCellHierarchy::nb_incompatible_poses(LabelledImagePool *pool) {
 324   PoseCell target_cell;
 325   PoseCellSet cell_set;
 326   LabelledImage *image;
 327
 328   int nb_errors = 0;
 329
 330   for(int i = 0; i < pool->nb_images(); i++) {
 331     image = pool->grab_image(i);
 332
 333     for(int t = 0; t < image->nb_targets(); t++) {
 334       cell_set.erase_content();
 335
 336       int error_level = -1;
 337
 338       for(int l = 0; error_level < 0 && l < _nb_levels; l++) {
 339         cell_set.erase_content();
 340
 341         if(l == 0) {
 342           add_root_cells(image, &cell_set);
 343         } else {
 344           add_subcells(l, &target_cell, &cell_set);
 345         }
 346
 347         int nb_compliant = 0;
 348
 349         for(int c = 0; c < cell_set.nb_cells(); c++) {
 350           if(cell_set.get_cell(c)->contains(image->get_target_pose(t))) {
 351             target_cell = *(cell_set.get_cell(c));
 352             nb_compliant++;
 353           }
 354         }
 355
 356         if(nb_compliant != 1) {
 357           error_level = l;
 358         }
 359       }
 360
 361       if(error_level >= 0) {
 362         nb_errors++;
 363       }
 364     }
 365
 366     pool->release_image(i);
 367   }
 368
 369   return nb_errors;
 370 }
 371
 372 void PoseCellHierarchy::write(ostream *os) {
 373   write_var(os, &_min_head_radius);
 374   write_var(os, &_max_head_radius);
 375   write_var(os, &_root_cell_nb_xy_per_scale);
 376   write_var(os, &_nb_body_cells);
 377   for(int k = 0; k < _nb_body_cells; k++)
 378     write_var(os, &_body_cells[k]);
 379 }
 380
 381 void PoseCellHierarchy::read(istream *is) {
 382   delete[] _body_cells;
 383   read_var(is, &_min_head_radius);
 384   read_var(is, &_max_head_radius);
 385   read_var(is, &_root_cell_nb_xy_per_scale);
 386   read_var(is, &_nb_body_cells);
 387   delete[] _body_cells;
 388   _body_cells = new RelativeBodyPoseCell[_nb_body_cells];
 389   for(int k = 0; k < _nb_body_cells; k++) {
 390     read_var(is, &_body_cells[k]);
 391   }
 392 }