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. //
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. //
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/>. //
15 // Written by Francois Fleuret, (C) IDIAP //
16 // Contact <francois.fleuret@idiap.ch> for comments & bug reports //
17 ///////////////////////////////////////////////////////////////////////////
19 #include "pi_referential.h"
21 #include "rich_image.h"
23 void PiReferential::draw_frame(RGBImage *image,
24 int registration_mode,
32 switch(registration_mode) {
34 case PiReferential::RM_HEAD:
35 r = 0; g = 255; b = 0;
38 case PiReferential::RM_HEAD_NO_POLARITY:
39 r = 128; g = 255; b = 128;
42 case PiReferential::RM_BELLY:
43 r = 64; g = 0; b = 255;
46 case PiReferential::RM_BELLY_NO_POLARITY:
47 r = 192; g = 128; b = 255;
50 case PiReferential::RM_HEAD_BELLY:
51 case PiReferential::RM_HEAD_BELLY_EDGES:
52 r = 255; g = 0; b = 0;
56 cerr << "INCONSISTENCY" << endl;
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);
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);
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
90 void PiReferential::draw_window(RGBImage *image,
91 int registration_mode, Rectangle *window,
95 switch(registration_mode) {
97 case PiReferential::RM_HEAD:
98 r = 0; g = 255; b = 0;
101 case PiReferential::RM_HEAD_NO_POLARITY:
102 r = 128; g = 255; b = 128;
105 case PiReferential::RM_BELLY:
106 r = 64; g = 0; b = 255;
109 case PiReferential::RM_BELLY_NO_POLARITY:
110 r = 192; g = 128; b = 255;
113 case PiReferential::RM_HEAD_BELLY:
114 case PiReferential::RM_HEAD_BELLY_EDGES:
115 r = 255; g = 0; b = 0;
119 cerr << "INCONSISTENCY" << endl;
123 int xmin = int(window->xmin);
124 int ymin = int(window->ymin);
125 int xmax = int(window->xmax);
126 int ymax = int(window->ymax);
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);
137 // for(int d = ymin - ymax; d <= xmax - xmin; d += delta) {
138 // int x1 = xmin + d;
140 // int x2 = xmin + d + ymax - ymin;
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);
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);
156 // for(int d = ymin - ymax; d <= xmax - xmin; d += delta) {
157 // int x1 = xmin + d;
159 // int x2 = xmin + d + ymax - ymin;
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);
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);
173 for(int d = ymin - ymax; d <= xmax - xmin; d += delta) {
176 int x2 = xmin + d + ymax - ymin;
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);
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;
194 switch(registration_mode) {
196 case PiReferential::RM_HEAD:
197 r = 0; g = 255; b = 0;
200 case PiReferential::RM_HEAD_NO_POLARITY:
201 r = 128; g = 255; b = 128;
204 case PiReferential::RM_BELLY:
205 r = 64; g = 0; b = 255;
208 case PiReferential::RM_BELLY_NO_POLARITY:
209 r = 192; g = 128; b = 255;
212 case PiReferential::RM_HEAD_BELLY_EDGES:
214 case PiReferential::RM_HEAD_BELLY:
215 r = 255; g = 0; b = 0;
219 cerr << "INCONSISTENCY" << endl;
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);
227 image->draw_ellipse(1, r, g, b, xc, yc, radius, radius, 0);
229 if(_tag >= RichImage::first_edge_tag && _tag < RichImage::first_edge_tag + RichImage::nb_edge_tags) {
233 switch(_tag - RichImage::first_edge_tag) {
270 scalar_t l = sqrt(dx * dx + dy * dy);
277 image->draw_ellipse(1, r, g, b, xc, yc, radius + delta, radius + delta, 0);
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));
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);
298 else if(_tag == RichImage::variance_tag) {
299 image->draw_ellipse(1, r, g, b, xc, yc, 8, 8, 0);
302 // else if(_tag >= RichImage::first_gray_tag && _tag < RichImage::first_gray_tag + RichImage::nb_gray_tags) {
306 PiReferential::PiReferential(PoseCell *cell) {
307 scalar_t head_radius = sqrt(scalar_t(cell->_head_radius.min * cell->_head_radius.max));
309 _common_scale = global.scale_to_discrete_log_scale(head_radius / global.min_head_radius);
311 scalar_t discrete_scale_ratio = global.discrete_log_scale_to_scale(_common_scale);
313 //////////////////////////////////////////////////////////////////////
314 // Locations and scales
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;
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);
329 if((_head_xc - _body_xc) * cos(_body_tilt) + (_head_yc - _body_yc) * sin(_body_tilt) > 0) {
335 const scalar_t belly_frame_factor = 2.0;
337 _belly_xc = _body_xc;
338 _belly_yc = _body_yc;
339 _belly_window_scaling = _head_window_scaling * belly_frame_factor;
341 // Head-belly location
343 _head_belly_xc = (_head_xc + _body_xc) * 0.5;
344 _head_belly_yc = (_head_yc + _body_yc) * 0.5;
346 //////////////////////////////////////////////////////////////////////
349 if(_body_xc >= _head_xc) {
350 _horizontal_polarity = 1;
352 _horizontal_polarity = -1;
357 if(_horizontal_polarity < 0) {
358 _head_ux = _head_radius * 2.0;
361 _head_ux = - _head_radius * 2.0;
366 _head_vy = - _head_radius * 2.0;
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;
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;
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;
387 _head_belly_ux = 2 * (_head_xc - _head_belly_xc);
388 _head_belly_uy = 2 * (_head_yc - _head_belly_yc);
390 if(_horizontal_polarity < 0) {
391 _head_belly_vx = _head_belly_uy;
392 _head_belly_vy = - _head_belly_ux;
394 _head_belly_vx = - _head_belly_uy;
395 _head_belly_vy = _head_belly_ux;
398 scalar_t l = sqrt(_head_belly_vx * _head_belly_vx + _head_belly_vy * _head_belly_vy);
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;
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;
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;
416 int PiReferential::common_scale() {
417 return _common_scale;
420 void PiReferential::register_rectangle(int registration_mode,
423 scalar_t alpha, beta , xc, yc, w, h;
425 alpha = (original->xmin + original->xmax) * 0.5;
426 beta = (original->ymin + original->ymax) * 0.5;
428 switch(registration_mode) {
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;
439 case RM_HEAD_NO_POLARITY:
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;
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;
457 case RM_BELLY_NO_POLARITY:
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;
467 case RM_HEAD_BELLY_EDGES:
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;
477 cerr << "Undefined registration mode." << endl;
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;
486 ASSERT(result->xmin < result->xmax && result->ymin < result->ymax);
489 int PiReferential::register_edge(int registration_mode, int edge_type) {
491 if(edge_type >= RichImage::first_edge_tag &&
492 edge_type < RichImage::first_edge_tag + RichImage::nb_edge_tags) {
494 int e = edge_type - RichImage::first_edge_tag;
496 switch(registration_mode) {
497 case PiReferential::RM_HEAD_NO_POLARITY:
498 case PiReferential::RM_BELLY_NO_POLARITY:
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;
509 case PiReferential::RM_HEAD_BELLY_EDGES:
510 if(_horizontal_polarity < 0) {
511 e = (RichImage::nb_edge_tags - e) % RichImage::nb_edge_tags;
513 e += _head_belly_edge_shift;
517 cerr << "INCONSISTENCY" << endl;
521 e = e % RichImage::nb_edge_tags;
523 return RichImage::first_edge_tag + e;
527 else return edge_type;
530 void PiReferential::draw(RGBImage *image, int level) {
531 int x1, y1, x2, y2, x3, y3, x4, y4;
535 // Draw the RM_BELLY reference frame
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);
546 draw_frame(image, RM_BELLY, x1, y1, x2, y2, x3, y3, x4, y4);
548 // Draw the RM_HEAD_BELLY reference frame
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);
559 draw_frame(image, RM_HEAD_BELLY, x1, y1, x2, y2, x3, y3, x4, y4);
562 // Draw the RM_HEAD reference frame
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);
573 draw_frame(image, RM_HEAD, x1, y1, x2, y2, x3, y3, x4, y4);
576 void PiReferential::print_registration_mode(ostream *out, int registration_mode) {
577 switch(registration_mode) {
581 case RM_HEAD_NO_POLARITY:
582 (*out) << "RM_HEAD_NO_POLARITY";
585 (*out) << "RM_BELLY";
587 case RM_BELLY_NO_POLARITY:
588 (*out) << "RM_BELLY_NO_POLARITY";
591 (*out) << "RM_HEAD_BELLY";
593 case RM_HEAD_BELLY_EDGES:
594 (*out) << "RM_HEAD_BELLY_EDGES";