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                                           //
  16 // (C) Idiap Research Institute                                          //
  17 //                                                                       //
  18 // Contact <francois.fleuret@idiap.ch> for comments & bug reports        //
  19 ///////////////////////////////////////////////////////////////////////////
  20
  21 #include "pose_cell_hierarchy.h"
  22 #include "gaussian.h"
  23
  24 PoseCellHierarchy::PoseCellHierarchy() {
  25   _belly_cells = 0;
  26 }
  27
  28 PoseCellHierarchy::PoseCellHierarchy(LabelledImagePool *train_pool) {
  29   _nb_levels = global.nb_levels;
  30   _min_head_radius = global.min_head_radius;
  31   _max_head_radius = global.max_head_radius;
  32   _root_cell_nb_xy_per_radius = global.root_cell_nb_xy_per_radius;
  33
  34   LabelledImage *image;
  35   int nb_total_targets = 0;
  36   for(int i = 0; i < train_pool->nb_images(); i++) {
  37     image = train_pool->grab_image(i);
  38     // We are going to symmetrize
  39     nb_total_targets += 2 * image->nb_targets();
  40     train_pool->release_image(i);
  41   }
  42
  43   RelativeBellyPoseCell targets[nb_total_targets];
  44
  45   int u = 0;
  46   for(int i = 0; i < train_pool->nb_images(); i++) {
  47     image = train_pool->grab_image(i);
  48
  49     PoseCellSet cell_set;
  50     add_root_cells(image, &cell_set);
  51
  52     for(int t = 0; t < image->nb_targets(); t++) {
  53       Pose pose = *image->get_target_pose(t);
  54       Pose coarse;
  55
  56       cell_set.get_containing_cell(&pose)->get_centroid(&coarse);
  57
  58       targets[u]._belly_xc.set((pose._belly_xc - coarse._head_xc) / coarse._head_radius);
  59       targets[u]._belly_yc.set((pose._belly_yc - coarse._head_yc) / coarse._head_radius);
  60       u++;
  61
  62       pose.horizontal_flip(image->width());
  63
  64       cell_set.get_containing_cell(&pose)->get_centroid(&coarse);
  65
  66       targets[u]._belly_xc.set((pose._belly_xc - coarse._head_xc) / coarse._head_radius);
  67       targets[u]._belly_yc.set((pose._belly_yc - coarse._head_yc) / coarse._head_radius);
  68       u++;
  69     }
  70
  71     train_pool->release_image(i);
  72   }
  73
  74   scalar_t fattening = 1.1;
  75
  76   Interval belly_rxc, belly_ryc;
  77
  78   belly_rxc.set(&targets[0]._belly_xc);
  79   belly_ryc.set(&targets[0]._belly_yc);
  80
  81   for(int t = 0; t < nb_total_targets; t++) {
  82     belly_rxc.swallow(&targets[t]._belly_xc);
  83     belly_ryc.swallow(&targets[t]._belly_yc);
  84   }
  85
  86   belly_rxc.min *= fattening;
  87   belly_rxc.max *= fattening;
  88   belly_ryc.min *= fattening;
  89   belly_ryc.max *= fattening;
  90
  91   scalar_t belly_rxc_min = belly_resolution * floor(belly_rxc.min / belly_resolution);
  92   int nb_belly_rxc = int(ceil((belly_rxc.max - belly_rxc_min) / belly_resolution));
  93
  94   scalar_t belly_ryc_min = belly_resolution * floor(belly_ryc.min / belly_resolution);
  95   int nb_belly_ryc = int(ceil((belly_ryc.max - belly_ryc_min) / belly_resolution));
  96
  97   int used[nb_belly_rxc * nb_belly_rxc];
  98
  99   for(int k = 0; k < nb_belly_rxc * nb_belly_ryc; k++) {
 100     used[k] = 1;
 101   }
 102
 103   // An ugly way to compute the convexe enveloppe
 104
 105   for(scalar_t alpha = 0; alpha < M_PI * 2; alpha += (2 * M_PI) / 100) {
 106     scalar_t vx = cos(alpha), vy = sin(alpha);
 107     scalar_t rho = 0;
 108
 109     for(int t = 0; t < nb_total_targets; t++) {
 110       rho = min(rho, vx * targets[t]._belly_xc.middle() + vy * targets[t]._belly_yc.middle());
 111     }
 112
 113     rho *= fattening;
 114
 115     for(int j = 0; j < nb_belly_ryc; j++) {
 116       for(int i = 0; i < nb_belly_rxc; i++) {
 117         if(
 118            vx * (scalar_t(i + 0) * belly_resolution + belly_rxc_min) +
 119            vy * (scalar_t(j + 0) * belly_resolution + belly_ryc_min) < rho
 120            &&
 121            vx * (scalar_t(i + 1) * belly_resolution + belly_rxc_min) +
 122            vy * (scalar_t(j + 0) * belly_resolution + belly_ryc_min) < rho
 123            &&
 124            vx * (scalar_t(i + 0) * belly_resolution + belly_rxc_min) +
 125            vy * (scalar_t(j + 1) * belly_resolution + belly_ryc_min) < rho
 126            &&
 127            vx * (scalar_t(i + 1) * belly_resolution + belly_rxc_min) +
 128            vy * (scalar_t(j + 1) * belly_resolution + belly_ryc_min) < rho
 129            ) {
 130           used[i + j * nb_belly_rxc] = 0;
 131         }
 132       }
 133     }
 134   }
 135
 136   _nb_belly_cells = 0;
 137   for(int j = 0; j < nb_belly_ryc; j++) {
 138     for(int i = 0; i < nb_belly_rxc; i++) {
 139       if(used[i + nb_belly_rxc * j]) {
 140         _nb_belly_cells++;
 141       }
 142     }
 143   }
 144
 145   _belly_cells = new RelativeBellyPoseCell[_nb_belly_cells];
 146
 147   int k = 0;
 148   for(int j = 0; j < nb_belly_ryc; j++) {
 149     for(int i = 0; i < nb_belly_rxc; i++) {
 150
 151       if(used[i + nb_belly_rxc * j]) {
 152
 153         RelativeBellyPoseCell mother;
 154
 155         scalar_t x = scalar_t(i) * belly_resolution + belly_rxc_min;
 156         scalar_t y = scalar_t(j) * belly_resolution + belly_ryc_min;
 157
 158         mother._belly_xc.set(x, x + belly_resolution);
 159         mother._belly_yc.set(y, y + belly_resolution);
 160
 161         _belly_cells[k++] = mother;
 162       }
 163     }
 164   }
 165 }
 166
 167 PoseCellHierarchy::~PoseCellHierarchy() {
 168   delete[] _belly_cells;
 169 }
 170
 171 int PoseCellHierarchy::nb_levels() {
 172   return _nb_levels;
 173 }
 174
 175 void PoseCellHierarchy::get_containing_cell(Image *image, int level,
 176                                             Pose *pose, PoseCell *result_cell) {
 177   PoseCellSet cell_set;
 178
 179   for(int l = 0; l < level + 1; l++) {
 180     cell_set.erase_content();
 181     if(l == 0) {
 182       add_root_cells(image, &cell_set);
 183     } else {
 184       add_subcells(l, result_cell, &cell_set);
 185     }
 186
 187     *result_cell = *(cell_set.get_containing_cell(pose));
 188   }
 189 }
 190
 191 void PoseCellHierarchy::add_root_cells(Image *image, PoseCellSet *cell_set) {
 192
 193   const int nb_scales = int((log(_max_head_radius) - log(_min_head_radius)) / log(2) *
 194                             global.nb_scales_per_power_of_two);
 195
 196   scalar_t alpha = log(_min_head_radius);
 197   scalar_t beta = log(2) / scalar_t(global.nb_scales_per_power_of_two);
 198
 199   for(int s = 0; s < nb_scales; s++) {
 200     scalar_t cell_xy_size = exp(alpha + scalar_t(s) * beta) / global.root_cell_nb_xy_per_radius;
 201     PoseCell cell;
 202     cell._head_radius.min = exp(alpha + scalar_t(s) * beta);
 203     cell._head_radius.max = exp(alpha + scalar_t(s+1) * beta);
 204     cell._head_tilt.min = -M_PI;
 205     cell._head_tilt.max = M_PI;
 206     for(scalar_t y = 0; y < image->height(); y += cell_xy_size)
 207       for(scalar_t x = 0; x < image->width(); x += cell_xy_size) {
 208         cell._head_xc.min = x;
 209         cell._head_xc.max = x + cell_xy_size;
 210         cell._head_yc.min = y;
 211         cell._head_yc.max = y + cell_xy_size;
 212         cell._belly_xc.min = cell._head_xc.min - pseudo_infty;
 213         cell._belly_xc.max = cell._head_xc.max + pseudo_infty;
 214         cell._belly_yc.min = cell._head_yc.min - pseudo_infty;
 215         cell._belly_yc.max = cell._head_yc.max + pseudo_infty;
 216         cell_set->add_cell(&cell);
 217       }
 218   }
 219 }
 220
 221 void PoseCellHierarchy::add_subcells(int level, PoseCell *root,
 222                                      PoseCellSet *cell_set) {
 223
 224   switch(level) {
 225
 226   case 1:
 227     {
 228       // Here we split the belly-center coordinate cell part
 229       PoseCell cell = *root;
 230       scalar_t r = sqrt(cell._head_radius.min * cell._head_radius.max);
 231       scalar_t x = (cell._head_xc.min + cell._head_xc.max) / 2.0;
 232       scalar_t y = (cell._head_yc.min + cell._head_yc.max) / 2.0;
 233       for(int k = 0; k < _nb_belly_cells; k++) {
 234         cell._belly_xc.min = (_belly_cells[k]._belly_xc.min * r) + x;
 235         cell._belly_xc.max = (_belly_cells[k]._belly_xc.max * r) + x;
 236         cell._belly_yc.min = (_belly_cells[k]._belly_yc.min * r) + y;
 237         cell._belly_yc.max = (_belly_cells[k]._belly_yc.max * r) + y;
 238         cell_set->add_cell(&cell);
 239       }
 240     }
 241     break;
 242
 243   default:
 244     {
 245       cerr << "Inconsistent level in PoseCellHierarchy::add_subcells" << endl;
 246       abort();
 247     }
 248     break;
 249   }
 250 }
 251
 252
 253 int PoseCellHierarchy::nb_incompatible_poses(LabelledImagePool *pool) {
 254   PoseCell target_cell;
 255   PoseCellSet cell_set;
 256   LabelledImage *image;
 257
 258   int nb_errors = 0;
 259
 260   for(int i = 0; i < pool->nb_images(); i++) {
 261     image = pool->grab_image(i);
 262
 263     for(int t = 0; t < image->nb_targets(); t++) {
 264       cell_set.erase_content();
 265
 266       int error_level = -1;
 267
 268       for(int l = 0; error_level < 0 && l < _nb_levels; l++) {
 269         cell_set.erase_content();
 270
 271         if(l == 0) {
 272           add_root_cells(image, &cell_set);
 273         } else {
 274           add_subcells(l, &target_cell, &cell_set);
 275         }
 276
 277         int nb_compliant = 0;
 278
 279         for(int c = 0; c < cell_set.nb_cells(); c++) {
 280           if(cell_set.get_cell(c)->contains(image->get_target_pose(t))) {
 281             target_cell = *(cell_set.get_cell(c));
 282             nb_compliant++;
 283           }
 284         }
 285
 286         if(nb_compliant != 1) {
 287           error_level = l;
 288         }
 289       }
 290
 291       if(error_level >= 0) {
 292         nb_errors++;
 293       }
 294     }
 295
 296     pool->release_image(i);
 297   }
 298
 299   return nb_errors;
 300 }
 301
 302 void PoseCellHierarchy::write(ostream *os) {
 303   write_var(os, &_min_head_radius);
 304   write_var(os, &_max_head_radius);
 305   write_var(os, &_root_cell_nb_xy_per_radius);
 306   write_var(os, &_nb_belly_cells);
 307   for(int k = 0; k < _nb_belly_cells; k++)
 308     write_var(os, &_belly_cells[k]);
 309 }
 310
 311 void PoseCellHierarchy::read(istream *is) {
 312   delete[] _belly_cells;
 313   read_var(is, &_min_head_radius);
 314   read_var(is, &_max_head_radius);
 315   read_var(is, &_root_cell_nb_xy_per_radius);
 316   read_var(is, &_nb_belly_cells);
 317   delete[] _belly_cells;
 318   _belly_cells = new RelativeBellyPoseCell[_nb_belly_cells];
 319   for(int k = 0; k < _nb_belly_cells; k++) {
 320     read_var(is, &_belly_cells[k]);
 321   }
 322 }