pose.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 <string.h>
  22
  23 #include "pose.h"
  24
  25 Pose::Pose() {
  26   memset(this, 0, sizeof(*this));
  27 }
  28
  29 void Pose::horizontal_flip(scalar_t scene_width) {
  30   _head_xc = scene_width - 1 - _head_xc;
  31   _belly_xc = scene_width - 1 - _belly_xc;
  32 }
  33
  34 const scalar_t tolerance_scale_ratio_for_hit = 1.5;
  35 const scalar_t tolerance_distance_factor_for_hit = 1.0;
  36
  37 bool Pose::hit(int level, Pose *pose) {
  38
  39   // Head size
  40
  41   if(_head_radius/pose->_head_radius > tolerance_scale_ratio_for_hit ||
  42      pose->_head_radius/_head_radius > tolerance_scale_ratio_for_hit)
  43     return false;
  44
  45   scalar_t sq_delta = _head_radius * pose->_head_radius * sq(tolerance_distance_factor_for_hit);
  46
  47   // Head location
  48
  49   if(sq(_head_xc - pose->_head_xc) + sq(_head_yc - pose->_head_yc) > sq_delta)
  50     return false;
  51
  52   if(level == 0) return true;
  53
  54   // Belly location
  55
  56   if(sq(_belly_xc - pose->_belly_xc) + sq(_belly_yc - pose->_belly_yc) > 4 * sq_delta)
  57     return false;
  58
  59   if(level == 1) return true;
  60
  61   cerr << "Hit criterion is undefined for level " << level << endl;
  62
  63   abort();
  64 }
  65
  66 const scalar_t tolerance_scale_ratio_for_collide = 1.2;
  67 const scalar_t tolerance_distance_factor_for_collide = 0.25;
  68
  69 bool Pose::collide(int level, Pose *pose) {
  70
  71   // Head size
  72
  73   if(_head_radius/pose->_head_radius > tolerance_scale_ratio_for_collide ||
  74      pose->_head_radius/_head_radius > tolerance_scale_ratio_for_collide)
  75     return false;
  76
  77   scalar_t sq_delta = _head_radius * pose->_head_radius * sq(tolerance_distance_factor_for_collide);
  78
  79   // Head location
  80
  81   if(sq(_head_xc - pose->_head_xc) + sq(_head_yc - pose->_head_yc) <= sq_delta)
  82     return true;
  83
  84   if(level == 0) return false;
  85
  86   // Belly location
  87
  88   if(sq(_belly_xc - pose->_belly_xc) + sq(_belly_yc - pose->_belly_yc) <= sq_delta)
  89     return true;
  90
  91   if(level == 1) return false;
  92
  93   cerr << "Colliding criterion is undefined for level " << level << endl;
  94
  95   abort();
  96 }
  97
  98 void Pose::draw(int thickness, int r, int g, int b, int level, RGBImage *image) {
  99   // Draw the head circle
 100
 101   image->draw_ellipse(thickness, r, g, b,
 102                       _head_xc, _head_yc, _head_radius, _head_radius, 0);
 103
 104   if(level == 1) {
 105
 106     scalar_t vx = _belly_xc - _head_xc, vy = _belly_yc - _head_yc;
 107     scalar_t l = sqrt(vx * vx + vy * vy);
 108     vx /= l;
 109     vy /= l;
 110
 111     scalar_t belly_radius = 12 + thickness / 2;
 112
 113     if(l > _head_radius + thickness + belly_radius) {
 114       image->draw_line(thickness, r, g, b,
 115                        _head_xc + vx * (_head_radius + thickness/2),
 116                        _head_yc + vy * (_head_radius + thickness/2),
 117                        _belly_xc - vx * (belly_radius - thickness/2),
 118                        _belly_yc - vy * (belly_radius - thickness/2));
 119     }
 120
 121     // An ugly way to make a filled disc
 122     for(scalar_t u = 0; u < belly_radius; u += thickness / 2) {
 123       image->draw_ellipse(thickness, r, g, b, _belly_xc, _belly_yc, u, u, 0);
 124     }
 125
 126   }
 127 }
 128
 129 void Pose::print(ostream *out) {
 130   (*out) << "  _head_xc " << _head_xc << endl;
 131   (*out) << "  _head_yc " << _head_yc << endl;
 132   (*out) << "  _head_radius " << _head_radius << endl;
 133   (*out) << "  _belly_xc " << _belly_xc << endl;
 134   (*out) << "  _belly_yc " << _belly_yc << endl;
 135 }
 136
 137 void Pose::write(ostream *out) {
 138   write_var(out, &_bounding_box_xmin);
 139   write_var(out, &_bounding_box_ymin);
 140   write_var(out, &_bounding_box_xmax);
 141   write_var(out, &_bounding_box_ymax);
 142   write_var(out, &_head_xc); write_var(out, &_head_yc);
 143   write_var(out, &_head_radius);
 144   write_var(out, &_belly_xc);
 145   write_var(out, &_belly_yc);
 146 }
 147
 148 void Pose::read(istream *in) {
 149   read_var(in, &_bounding_box_xmin);
 150   read_var(in, &_bounding_box_ymin);
 151   read_var(in, &_bounding_box_xmax);
 152   read_var(in, &_bounding_box_ymax);
 153   read_var(in, &_head_xc); read_var(in, &_head_yc);
 154   read_var(in, &_head_radius);
 155   read_var(in, &_belly_xc);
 156   read_var(in, &_belly_yc);
 157 }