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, (C) IDIAP                                //
  16 // Contact <francois.fleuret@idiap.ch> for comments & bug reports        //
  17 ///////////////////////////////////////////////////////////////////////////
  18
  19 #include <string.h>
  20
  21 #include "pose.h"
  22
  23 Pose::Pose() {
  24   memset(this, 0, sizeof(*this));
  25 }
  26
  27 void Pose::horizontal_flip(scalar_t scene_width) {
  28   _head_xc = scene_width - 1 - _head_xc;
  29   _body_xc = scene_width - 1 - _body_xc;
  30 }
  31
  32 void Pose::translate(scalar_t dx, scalar_t dy) {
  33   _bounding_box_xmin += dx;
  34   _bounding_box_ymin += dy;
  35   _bounding_box_xmax += dx;
  36   _bounding_box_ymax += dy;
  37   _head_xc += dx;
  38   _head_yc += dy;
  39   _body_xc += dx;
  40   _body_yc += dy;
  41 }
  42
  43 void Pose::scale(scalar_t factor) {
  44   _bounding_box_xmin *= factor;
  45   _bounding_box_ymin *= factor;
  46   _bounding_box_xmax *= factor;
  47   _bounding_box_ymax *= factor;
  48   _head_xc *= factor;
  49   _head_yc *= factor;
  50   _head_radius *= factor;
  51   _body_xc *= factor;
  52   _body_yc *= factor;
  53 }
  54
  55 const scalar_t tolerance_scale_ratio_for_hit = 1.5;
  56 const scalar_t tolerance_distance_factor_for_hit = 1.0;
  57
  58 bool Pose::hit(int level, Pose *pose) {
  59
  60   // Head size
  61
  62   if(_head_radius/pose->_head_radius > tolerance_scale_ratio_for_hit ||
  63      pose->_head_radius/_head_radius > tolerance_scale_ratio_for_hit)
  64     return false;
  65
  66   scalar_t sq_delta = _head_radius * pose->_head_radius * sq(tolerance_distance_factor_for_hit);
  67
  68   // Head location
  69
  70   if(sq(_head_xc - pose->_head_xc) + sq(_head_yc - pose->_head_yc) > sq_delta)
  71     return false;
  72
  73   if(level == 0) return true;
  74
  75   // Belly location
  76
  77   if(sq(_body_xc - pose->_body_xc) + sq(_body_yc - pose->_body_yc) > 4 * sq_delta)
  78     return false;
  79
  80   if(level == 1) return true;
  81
  82   cerr << "Hit criterion is undefined for level " << level << endl;
  83
  84   abort();
  85 }
  86
  87 const scalar_t tolerance_scale_ratio_for_collide = 1.2;
  88 const scalar_t tolerance_distance_factor_for_collide = 0.25;
  89
  90 bool Pose::collide(int level, Pose *pose) {
  91
  92   // Head size
  93
  94   if(_head_radius/pose->_head_radius > tolerance_scale_ratio_for_collide ||
  95      pose->_head_radius/_head_radius > tolerance_scale_ratio_for_collide)
  96     return false;
  97
  98   scalar_t sq_delta = _head_radius * pose->_head_radius * sq(tolerance_distance_factor_for_collide);
  99
 100   // Head location
 101
 102   if(sq(_head_xc - pose->_head_xc) + sq(_head_yc - pose->_head_yc) <= sq_delta)
 103     return true;
 104
 105   if(level == 0) return false;
 106
 107   // Belly location
 108
 109   if(sq(_body_xc - pose->_body_xc) + sq(_body_yc - pose->_body_yc) <= sq_delta)
 110     return true;
 111
 112   if(level == 1) return false;
 113
 114   cerr << "Colliding criterion is undefined for level " << level << endl;
 115
 116   abort();
 117 }
 118
 119 void Pose::draw(int thickness, int r, int g, int b, int level, RGBImage *image) {
 120   // Draw the head circle
 121
 122   image->draw_ellipse(thickness, r, g, b,
 123                       _head_xc, _head_yc, _head_radius, _head_radius, 0);
 124
 125   //   int vx = int(cos(_head_tilt) * _head_radius);
 126   //   int vy = int(sin(_head_tilt) * _head_radius);
 127   //   image->draw_line(thickness, r, g, b, _head_xc, _head_yc, _head_xc + vx, _head_yc + vy);
 128
 129   if(level == 1) {
 130
 131     //     image->draw_line(thickness, r, g, b,
 132     //                      int(_body_xc) - delta, int(_body_yc),
 133     //                      int(_body_xc) + delta, int(_body_yc));
 134
 135     //     image->draw_line(thickness, r, g, b,
 136     //                      int(_body_xc), int(_body_yc) - delta,
 137     //                      int(_body_xc), int(_body_yc) + delta);
 138
 139     scalar_t vx = _body_xc - _head_xc, vy = _body_yc - _head_yc;
 140     scalar_t l = sqrt(vx * vx + vy * vy);
 141     vx /= l;
 142     vy /= l;
 143
 144     scalar_t body_radius = 12 + thickness / 2;
 145
 146     if(l > _head_radius + thickness + body_radius) {
 147       image->draw_line(thickness, r, g, b,
 148                        _head_xc + vx * (_head_radius + thickness/2),
 149                        _head_yc + vy * (_head_radius + thickness/2),
 150                        _body_xc - vx * (body_radius - thickness/2),
 151                        _body_yc - vy * (body_radius - thickness/2));
 152     }
 153
 154     // An ugly way to make a filled disc
 155     for(scalar_t u = 0; u < body_radius; u += thickness / 2) {
 156       image->draw_ellipse(thickness, r, g, b, _body_xc, _body_yc, u, u, 0);
 157     }
 158
 159   }
 160 }
 161
 162 void Pose::print(ostream *out) {
 163   (*out) << "  _head_xc " << _head_xc << endl;
 164   (*out) << "  _head_yc " << _head_yc << endl;
 165   (*out) << "  _head_radius " << _head_radius << endl;
 166   (*out) << "  _body_xc " << _body_xc << endl;
 167   (*out) << "  _body_yc " << _body_yc << endl;
 168 }
 169
 170 void Pose::write(ostream *out) {
 171   write_var(out, &_bounding_box_xmin);
 172   write_var(out, &_bounding_box_ymin);
 173   write_var(out, &_bounding_box_xmax);
 174   write_var(out, &_bounding_box_ymax);
 175   write_var(out, &_head_xc); write_var(out, &_head_yc);
 176   write_var(out, &_head_radius);
 177   write_var(out, &_body_xc);
 178   write_var(out, &_body_yc);
 179 }
 180
 181 void Pose::read(istream *in) {
 182   read_var(in, &_bounding_box_xmin);
 183   read_var(in, &_bounding_box_ymin);
 184   read_var(in, &_bounding_box_xmax);
 185   read_var(in, &_bounding_box_ymax);
 186   read_var(in, &_head_xc); read_var(in, &_head_yc);
 187   read_var(in, &_head_radius);
 188   read_var(in, &_body_xc);
 189   read_var(in, &_body_yc);
 190 }