automatic commit
[folded-ctf.git] / 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     //   int xc = (x1 + x2 + x3 + x4)/4, yc = (y1 + y2 + y3 + y4)/4;
74     //     image->draw_line(1, r, g, b, xc - delta, yc, xc + delta, yc);
75     //     image->draw_line(1, r, g, b, xc, yc - delta, xc, yc + delta);
76     image->draw_line(2, r, g, b, x1, y1, x2, y2);
77     image->draw_line(2, r, g, b, x2, y2, x3, y3);
78     image->draw_line(2, r, g, b, x3, y3, x4, y4);
79     image->draw_line(2, r, g, b, x4, y4, x1, y1);
80     //     image->draw_line(2, r, g, b,
81     //                      (2*xc + 5 * x1 + 5 * x2)/12, (2 * yc + 5 * y1 + 5 * y2)/12,
82     //                      (x1 + x2)/2, (y1 + y2)/2);
83     //     image->draw_line(6, r, g, b,
84     //                      (2*xc + 3 * x2 + 3 * x3)/8, (2 * yc + 3 * y2 + 3 * y3)/8,
85     //                      (x2 + x3)/2, (y2 + y3)/2
86     //                      );
87   }
88 }
89
90 void PiReferential::draw_window(RGBImage *image,
91                                 int registration_mode, Rectangle *window,
92                                 int filled) {
93   int r, g, b;
94
95   switch(registration_mode) {
96
97   case PiReferential::RM_HEAD:
98     r = 0; g = 255; b = 0;
99     break;
100
101   case PiReferential::RM_HEAD_NO_POLARITY:
102     r = 128; g = 255; b = 128;
103     break;
104
105   case PiReferential::RM_BELLY:
106     r = 64; g = 0; b = 255;
107     break;
108
109   case PiReferential::RM_BELLY_NO_POLARITY:
110     r = 192; g = 128; b = 255;
111     break;
112
113   case PiReferential::RM_HEAD_BELLY:
114   case PiReferential::RM_HEAD_BELLY_EDGES:
115     r = 255; g = 0; b = 0;
116     break;
117
118   default:
119     cerr << "INCONSISTENCY" << endl;
120     abort();
121   }
122
123   int xmin = int(window->xmin);
124   int ymin = int(window->ymin);
125   int xmax = int(window->xmax);
126   int ymax = int(window->ymax);
127
128   if(global.pictures_for_article) {
129     r = 255; g = 255; b = 255;
130     image->draw_line(6, r, g, b, xmin, ymin, xmax, ymin);
131     image->draw_line(6, r, g, b, xmax, ymin, xmax, ymax);
132     image->draw_line(6, r, g, b, xmax, ymax, xmin, ymax);
133     image->draw_line(6, r, g, b, xmin, ymax, xmin, ymin);
134
135 //     if(filled) {
136 //       int delta = 6;
137 //       for(int d = ymin - ymax; d <= xmax - xmin; d += delta) {
138 //         int x1 = xmin + d;
139 //         int y1 = ymin;
140 //         int x2 = xmin + d + ymax - ymin;
141 //         int y2 = ymax;
142 //         if(x1 < xmin) { y1 = y1 + (xmin - x1); x1 = xmin; }
143 //         if(x2 > xmax) { y2 = y2 - (x2 - xmax); x2 = xmax; }
144 //         image->draw_line(3, r, g, b, x1, y1, x2, y2);
145 //       }
146 //     }
147
148     r =   0; g =   0; b =   0;
149     image->draw_line(2, r, g, b, xmin, ymin, xmax, ymin);
150     image->draw_line(2, r, g, b, xmax, ymin, xmax, ymax);
151     image->draw_line(2, r, g, b, xmax, ymax, xmin, ymax);
152     image->draw_line(2, r, g, b, xmin, ymax, xmin, ymin);
153
154 //     if(filled) {
155 //       int delta = 6;
156 //       for(int d = ymin - ymax; d <= xmax - xmin; d += delta) {
157 //         int x1 = xmin + d;
158 //         int y1 = ymin;
159 //         int x2 = xmin + d + ymax - ymin;
160 //         int y2 = ymax;
161 //         if(x1 < xmin) { y1 = y1 + (xmin - x1); x1 = xmin; }
162 //         if(x2 > xmax) { y2 = y2 - (x2 - xmax); x2 = xmax; }
163 //         image->draw_line(1, r, g, b, x1, y1, x2, y2);
164 //       }
165 //     }
166   } else {
167     image->draw_line(2, r, g, b, xmin, ymin, xmax, ymin);
168     image->draw_line(2, r, g, b, xmax, ymin, xmax, ymax);
169     image->draw_line(2, r, g, b, xmax, ymax, xmin, ymax);
170     image->draw_line(2, r, g, b, xmin, ymax, xmin, ymin);
171     if(filled) {
172       int delta = 4;
173       for(int d = ymin - ymax; d <= xmax - xmin; d += delta) {
174         int x1 = xmin + d;
175         int y1 = ymin;
176         int x2 = xmin + d + ymax - ymin;
177         int y2 = ymax;
178         if(x1 < xmin) { y1 = y1 + (xmin - x1); x1 = xmin; }
179         if(x2 > xmax) { y2 = y2 - (x2 - xmax); x2 = xmax; }
180         image->draw_line(1, r, g, b, x1, y1, x2, y2);
181       }
182     }
183   }
184
185 }
186
187 void PiReferential::draw_edge_and_scale(RGBImage *image,
188                                         int registration_mode, Rectangle *window,
189                                         int _tag, int _edge_scale) {
190   const int ref_radius = 10;
191   int r, g, b;
192   int edges = 0;
193
194   switch(registration_mode) {
195
196   case PiReferential::RM_HEAD:
197     r = 0; g = 255; b = 0;
198     break;
199
200   case PiReferential::RM_HEAD_NO_POLARITY:
201     r = 128; g = 255; b = 128;
202     break;
203
204   case PiReferential::RM_BELLY:
205     r = 64; g = 0; b = 255;
206     break;
207
208   case PiReferential::RM_BELLY_NO_POLARITY:
209     r = 192; g = 128; b = 255;
210     break;
211
212   case PiReferential::RM_HEAD_BELLY_EDGES:
213     edges = 1;
214   case PiReferential::RM_HEAD_BELLY:
215     r = 255; g = 0; b = 0;
216     break;
217
218   default:
219     cerr << "INCONSISTENCY" << endl;
220     abort();
221   }
222
223   scalar_t xc = (window->xmin + window->xmax)/2;
224   scalar_t yc = (window->ymin + window->ymax)/2;
225   int radius = ref_radius * (1 << _edge_scale);
226
227   image->draw_ellipse(1, r, g, b, xc, yc, radius, radius, 0);
228
229   if(_tag >= RichImage::first_edge_tag && _tag < RichImage::first_edge_tag + RichImage::nb_edge_tags) {
230
231     scalar_t dx, dy;
232
233     switch(_tag - RichImage::first_edge_tag) {
234     case 0:
235       dx =  0; dy = -1;
236       break;
237
238     case 1:
239       dx =  1; dy = -1;
240       break;
241
242     case 2:
243       dx =  1; dy =  0;
244       break;
245
246     case 3:
247       dx =  1; dy =  1;
248       break;
249
250     case 4:
251       dx =  0; dy =  1;
252       break;
253
254     case 5:
255       dx = -1; dy =  1;
256       break;
257
258     case 6:
259       dx = -1; dy =  0;
260       break;
261
262     case 7:
263       dx = -1; dy = -1;
264       break;
265
266     default:
267       abort();
268     }
269
270     scalar_t l = sqrt(dx * dx + dy * dy);
271
272 //     dx = dx / l;
273 //     dy = dy / l;
274
275     if(edges) {
276       int delta = 3;
277       image->draw_ellipse(1, r, g, b, xc, yc, radius + delta, radius + delta, 0);
278     }
279
280     for(scalar_t u = 0; u <= radius; u += 0.1) {
281       scalar_t s = sqrt(radius * radius - (u * u * l * l))/l;
282       image->draw_line(2, r, g, b,
283                        int(xc + u * dx - s * dy), int(yc + u * dy + s * dx),
284                        int(xc + u * dx + s * dy), int(yc + u * dy - s * dx));
285     }
286
287 //     for(int y = yc - radius; y <= yc + radius; y++) {
288 //       for(int x = xc - radius; x <= xc + radius; x++) {
289 //         if(x >= 0 && x < image->width() && y >= 0 && y < image->height() &&
290 //            (x - xc) * dx + (y - yc) * dy >= 0) {
291 //           image->draw_point(r, g, b, x, y);
292 //         }
293 //       }
294 //     }
295
296   }
297
298   else if(_tag == RichImage::variance_tag) {
299     image->draw_ellipse(1, r, g, b, xc, yc, 8, 8, 0);
300   }
301
302   //   else if(_tag >= RichImage::first_gray_tag && _tag < RichImage::first_gray_tag + RichImage::nb_gray_tags) {
303   //   }
304 }
305
306 PiReferential::PiReferential(PoseCell *cell) {
307   scalar_t head_radius = sqrt(scalar_t(cell->_head_radius.min * cell->_head_radius.max));
308
309   _common_scale = global.scale_to_discrete_log_scale(head_radius / global.min_head_radius);
310
311   scalar_t discrete_scale_ratio = global.discrete_log_scale_to_scale(_common_scale);
312
313   //////////////////////////////////////////////////////////////////////
314   // Locations and scales
315
316   // Head location
317
318   _head_xc = cell->_head_xc.middle() * discrete_scale_ratio;
319   _head_yc = cell->_head_yc.middle() * discrete_scale_ratio;
320   _head_radius = cell->_head_radius.middle() * discrete_scale_ratio;
321   _head_window_scaling = _head_radius * 2.0;
322
323   // Body location
324
325   _body_xc = cell->_belly_xc.middle() * discrete_scale_ratio;
326   _body_yc = cell->_belly_yc.middle() * discrete_scale_ratio;
327   _body_window_scaling = sqrt(_body_radius_1 * _body_radius_2);
328
329   if((_head_xc - _body_xc) * cos(_body_tilt) + (_head_yc - _body_yc) * sin(_body_tilt) > 0) {
330     _body_tilt += M_PI;
331   }
332
333   // Belly location
334
335   const scalar_t belly_frame_factor = 2.0;
336
337   _belly_xc = _body_xc;
338   _belly_yc = _body_yc;
339   _belly_window_scaling = _head_window_scaling * belly_frame_factor;
340
341   // Head-belly location
342
343   _head_belly_xc = (_head_xc + _body_xc) * 0.5;
344   _head_belly_yc = (_head_yc + _body_yc) * 0.5;
345
346   //////////////////////////////////////////////////////////////////////
347   // Frames
348
349   if(_body_xc >= _head_xc) {
350     _horizontal_polarity = 1;
351   } else {
352     _horizontal_polarity = -1;
353   }
354
355   // Head frame
356
357   if(_horizontal_polarity < 0) {
358     _head_ux = _head_radius * 2.0;
359     _head_uy = 0;
360   } else {
361     _head_ux = - _head_radius * 2.0;
362     _head_uy = 0;
363   }
364
365   _head_vx = 0;
366   _head_vy = - _head_radius * 2.0;
367
368   _head_ux_nopolarity = _head_radius * 2.0;
369   _head_uy_nopolarity = 0;
370   _head_vx_nopolarity = 0;
371   _head_vy_nopolarity = - _head_radius * 2.0;
372
373   // Belly frame
374
375   _belly_ux = _head_ux * belly_frame_factor;
376   _belly_uy = _head_uy * belly_frame_factor;
377   _belly_vx = _head_vx * belly_frame_factor;
378   _belly_vy = _head_vy * belly_frame_factor;
379
380   _belly_ux_nopolarity = _head_ux_nopolarity * belly_frame_factor;
381   _belly_uy_nopolarity = _head_uy_nopolarity * belly_frame_factor;
382   _belly_vx_nopolarity = _head_vx_nopolarity * belly_frame_factor;
383   _belly_vy_nopolarity = _head_vy_nopolarity * belly_frame_factor;
384
385   // Head-belly frame
386
387   _head_belly_ux = 2 * (_head_xc - _head_belly_xc);
388   _head_belly_uy = 2 * (_head_yc - _head_belly_yc);
389
390   if(_horizontal_polarity < 0) {
391     _head_belly_vx =   _head_belly_uy;
392     _head_belly_vy = - _head_belly_ux;
393   } else {
394     _head_belly_vx = - _head_belly_uy;
395     _head_belly_vy =   _head_belly_ux;
396   }
397
398   scalar_t l = sqrt(_head_belly_vx * _head_belly_vx + _head_belly_vy * _head_belly_vy);
399
400   _head_belly_vx = _head_belly_vx/l * _head_radius * 2;
401   _head_belly_vy = _head_belly_vy/l * _head_radius * 2;
402   _head_belly_edge_shift = int(floor(- RichImage::nb_edge_tags * atan2(_head_belly_ux, _head_belly_uy) / (2 * M_PI) + 0.5));
403   _head_belly_edge_shift = (RichImage::nb_edge_tags + _head_belly_edge_shift) % RichImage::nb_edge_tags;
404
405   // Body frame
406
407   _body_ux =   cos(_body_tilt) * _body_radius_1 * 2.0;
408   _body_uy =   sin(_body_tilt) * _body_radius_1 * 2.0;
409   _body_vx = - sin(_body_tilt) * _body_radius_2 * 2.0;
410   _body_vy =   cos(_body_tilt) * _body_radius_2 * 2.0;
411
412   _body_edge_shift = int(floor(RichImage::nb_edge_tags * _body_tilt / (2 * M_PI) + 0.5));
413   _body_edge_shift = (RichImage::nb_edge_tags + _body_edge_shift) % RichImage::nb_edge_tags;
414 }
415
416 int PiReferential::common_scale() {
417   return _common_scale;
418 }
419
420 void PiReferential::register_rectangle(int registration_mode,
421                                        Rectangle *original,
422                                        Rectangle *result) {
423   scalar_t alpha, beta , xc, yc, w, h;
424
425   alpha = (original->xmin + original->xmax) * 0.5;
426   beta  = (original->ymin + original->ymax) * 0.5;
427
428   switch(registration_mode) {
429
430   case RM_HEAD:
431     {
432       xc = _head_xc + alpha * _head_ux + beta * _head_vx;
433       yc = _head_yc + alpha * _head_uy + beta * _head_vy;
434       w = (original->xmax - original->xmin) * _head_window_scaling;
435       h = (original->ymax - original->ymin) * _head_window_scaling;
436     }
437     break;
438
439   case RM_HEAD_NO_POLARITY:
440     {
441       xc = _head_xc + alpha * _head_ux_nopolarity + beta * _head_vx_nopolarity;
442       yc = _head_yc + alpha * _head_uy_nopolarity + beta * _head_vy_nopolarity;
443       w = (original->xmax - original->xmin) * _head_window_scaling;
444       h = (original->ymax - original->ymin) * _head_window_scaling;
445     }
446     break;
447
448   case RM_BELLY:
449     {
450       xc = _belly_xc + alpha * _belly_ux + beta * _belly_vx;
451       yc = _belly_yc + alpha * _belly_uy + beta * _belly_vy;
452       w = (original->xmax - original->xmin) * _belly_window_scaling;
453       h = (original->ymax - original->ymin) * _belly_window_scaling;
454     }
455     break;
456
457   case RM_BELLY_NO_POLARITY:
458     {
459       xc = _belly_xc + alpha * _belly_ux_nopolarity + beta * _belly_vx_nopolarity;
460       yc = _belly_yc + alpha * _belly_uy_nopolarity + beta * _belly_vy_nopolarity;
461       w = (original->xmax - original->xmin) * _belly_window_scaling;
462       h = (original->ymax - original->ymin) * _belly_window_scaling;
463     }
464     break;
465
466   case RM_HEAD_BELLY:
467   case RM_HEAD_BELLY_EDGES:
468     {
469       xc = _head_belly_xc + alpha * _head_belly_ux + beta * _head_belly_vx;
470       yc = _head_belly_yc + alpha * _head_belly_uy + beta * _head_belly_vy;
471       w = (original->xmax - original->xmin) * _head_window_scaling;
472       h = (original->ymax - original->ymin) * _head_window_scaling;
473     }
474     break;
475
476   default:
477     cerr << "Undefined registration mode." << endl;
478     abort();
479   }
480
481   result->xmin = xc - 0.5 * w;
482   result->ymin = yc - 0.5 * h;
483   result->xmax = xc + 0.5 * w;
484   result->ymax = yc + 0.5 * h;
485
486   ASSERT(result->xmin < result->xmax && result->ymin < result->ymax);
487 }
488
489 int PiReferential::register_edge(int registration_mode, int edge_type) {
490
491   if(edge_type >= RichImage::first_edge_tag &&
492      edge_type < RichImage::first_edge_tag + RichImage::nb_edge_tags) {
493
494     int e = edge_type - RichImage::first_edge_tag;
495
496     switch(registration_mode) {
497     case PiReferential::RM_HEAD_NO_POLARITY:
498     case PiReferential::RM_BELLY_NO_POLARITY:
499       break;
500
501     case PiReferential::RM_HEAD:
502     case PiReferential::RM_BELLY:
503     case PiReferential::RM_HEAD_BELLY:
504       if(_horizontal_polarity < 0) {
505         e = (RichImage::nb_edge_tags - e) % RichImage::nb_edge_tags;
506       }
507       break;
508
509     case PiReferential::RM_HEAD_BELLY_EDGES:
510       if(_horizontal_polarity < 0) {
511         e = (RichImage::nb_edge_tags - e) % RichImage::nb_edge_tags;
512       }
513       e += _head_belly_edge_shift;
514       break;
515
516     default:
517       cerr << "INCONSISTENCY" << endl;
518       abort();
519     }
520
521     e = e % RichImage::nb_edge_tags;
522
523     return RichImage::first_edge_tag + e;
524
525   }
526
527   else return edge_type;
528 }
529
530 void PiReferential::draw(RGBImage *image, int level) {
531   int x1, y1, x2, y2, x3, y3, x4, y4;
532
533   if(level >= 1) {
534
535     // Draw the RM_BELLY reference frame
536
537     x1 = int(_belly_xc + _belly_ux + _belly_vx);
538     y1 = int(_belly_yc + _belly_uy + _belly_vy);
539     x2 = int(_belly_xc - _belly_ux + _belly_vx);
540     y2 = int(_belly_yc - _belly_uy + _belly_vy);
541     x3 = int(_belly_xc - _belly_ux - _belly_vx);
542     y3 = int(_belly_yc - _belly_uy - _belly_vy);
543     x4 = int(_belly_xc + _belly_ux - _belly_vx);
544     y4 = int(_belly_yc + _belly_uy - _belly_vy);
545
546     draw_frame(image, RM_BELLY, x1, y1, x2, y2, x3, y3, x4, y4);
547
548     // Draw the RM_HEAD_BELLY reference frame
549
550     x1 = int(_head_belly_xc + _head_belly_ux + _head_belly_vx);
551     y1 = int(_head_belly_yc + _head_belly_uy + _head_belly_vy);
552     x2 = int(_head_belly_xc - _head_belly_ux + _head_belly_vx);
553     y2 = int(_head_belly_yc - _head_belly_uy + _head_belly_vy);
554     x3 = int(_head_belly_xc - _head_belly_ux - _head_belly_vx);
555     y3 = int(_head_belly_yc - _head_belly_uy - _head_belly_vy);
556     x4 = int(_head_belly_xc + _head_belly_ux - _head_belly_vx);
557     y4 = int(_head_belly_yc + _head_belly_uy - _head_belly_vy);
558
559     draw_frame(image, RM_HEAD_BELLY, x1, y1, x2, y2, x3, y3, x4, y4);
560   }
561
562   // Draw the RM_HEAD reference frame
563
564   x1 = int(_head_xc + _head_ux + _head_vx);
565   y1 = int(_head_yc + _head_uy + _head_vy);
566   x2 = int(_head_xc - _head_ux + _head_vx);
567   y2 = int(_head_yc - _head_uy + _head_vy);
568   x3 = int(_head_xc - _head_ux - _head_vx);
569   y3 = int(_head_yc - _head_uy - _head_vy);
570   x4 = int(_head_xc + _head_ux - _head_vx);
571   y4 = int(_head_yc + _head_uy - _head_vy);
572
573   draw_frame(image, RM_HEAD, x1, y1, x2, y2, x3, y3, x4, y4);
574 }
575
576 void PiReferential::print_registration_mode(ostream *out, int registration_mode) {
577   switch(registration_mode) {
578   case RM_HEAD:
579     (*out) << "RM_HEAD";
580     break;
581   case RM_HEAD_NO_POLARITY:
582     (*out) << "RM_HEAD_NO_POLARITY";
583     break;
584   case RM_BELLY:
585     (*out) << "RM_BELLY";
586     break;
587   case RM_BELLY_NO_POLARITY:
588     (*out) << "RM_BELLY_NO_POLARITY";
589     break;
590   case RM_HEAD_BELLY:
591     (*out) << "RM_HEAD_BELLY";
592     break;
593   case RM_HEAD_BELLY_EDGES:
594     (*out) << "RM_HEAD_BELLY_EDGES";
595     break;
596   default:
597     abort();
598   }
599 }