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;
55 case PiReferential::RM_BODY:
56 case PiReferential::RM_BODY_EDGES:
57 r = 0; g = 128; b = 255;
61 cerr << "INCONSISTENCY" << endl;
65 if(global.pictures_for_article) {
66 r = 255; g = 255; b = 255;
67 image->draw_line(6, r, g, b, x1, y1, x2, y2);
68 image->draw_line(6, r, g, b, x2, y2, x3, y3);
69 image->draw_line(6, r, g, b, x3, y3, x4, y4);
70 image->draw_line(6, r, g, b, x4, y4, x1, y1);
73 image->draw_line(2, r, g, b, x1, y1, x2, y2);
74 image->draw_line(2, r, g, b, x2, y2, x3, y3);
75 image->draw_line(2, r, g, b, x3, y3, x4, y4);
76 image->draw_line(2, r, g, b, x4, y4, x1, y1);
78 // int xc = (x1 + x2 + x3 + x4)/4, yc = (y1 + y2 + y3 + y4)/4;
79 // image->draw_line(1, r, g, b, xc - delta, yc, xc + delta, yc);
80 // image->draw_line(1, r, g, b, xc, yc - delta, xc, yc + delta);
81 image->draw_line(2, r, g, b, x1, y1, x2, y2);
82 image->draw_line(2, r, g, b, x2, y2, x3, y3);
83 image->draw_line(2, r, g, b, x3, y3, x4, y4);
84 image->draw_line(2, r, g, b, x4, y4, x1, y1);
85 // image->draw_line(2, r, g, b,
86 // (2*xc + 5 * x1 + 5 * x2)/12, (2 * yc + 5 * y1 + 5 * y2)/12,
87 // (x1 + x2)/2, (y1 + y2)/2);
88 // image->draw_line(6, r, g, b,
89 // (2*xc + 3 * x2 + 3 * x3)/8, (2 * yc + 3 * y2 + 3 * y3)/8,
90 // (x2 + x3)/2, (y2 + y3)/2
95 void PiReferential::draw_window(RGBImage *image,
96 int registration_mode, Rectangle *window,
100 switch(registration_mode) {
102 case PiReferential::RM_HEAD:
103 r = 0; g = 255; b = 0;
106 case PiReferential::RM_HEAD_NO_POLARITY:
107 r = 128; g = 255; b = 128;
110 case PiReferential::RM_BELLY:
111 r = 64; g = 0; b = 255;
114 case PiReferential::RM_BELLY_NO_POLARITY:
115 r = 192; g = 128; b = 255;
118 case PiReferential::RM_HEAD_BELLY:
119 case PiReferential::RM_HEAD_BELLY_EDGES:
120 r = 255; g = 0; b = 0;
123 case PiReferential::RM_BODY:
124 case PiReferential::RM_BODY_EDGES:
125 r = 0; g = 128; b = 255;
129 cerr << "INCONSISTENCY" << endl;
133 int xmin = int(window->xmin);
134 int ymin = int(window->ymin);
135 int xmax = int(window->xmax);
136 int ymax = int(window->ymax);
138 if(global.pictures_for_article) {
139 r = 255; g = 255; b = 255;
140 image->draw_line(6, r, g, b, xmin, ymin, xmax, ymin);
141 image->draw_line(6, r, g, b, xmax, ymin, xmax, ymax);
142 image->draw_line(6, r, g, b, xmax, ymax, xmin, ymax);
143 image->draw_line(6, r, g, b, xmin, ymax, xmin, ymin);
147 // for(int d = ymin - ymax; d <= xmax - xmin; d += delta) {
148 // int x1 = xmin + d;
150 // int x2 = xmin + d + ymax - ymin;
152 // if(x1 < xmin) { y1 = y1 + (xmin - x1); x1 = xmin; }
153 // if(x2 > xmax) { y2 = y2 - (x2 - xmax); x2 = xmax; }
154 // image->draw_line(3, r, g, b, x1, y1, x2, y2);
159 image->draw_line(2, r, g, b, xmin, ymin, xmax, ymin);
160 image->draw_line(2, r, g, b, xmax, ymin, xmax, ymax);
161 image->draw_line(2, r, g, b, xmax, ymax, xmin, ymax);
162 image->draw_line(2, r, g, b, xmin, ymax, xmin, ymin);
166 // for(int d = ymin - ymax; d <= xmax - xmin; d += delta) {
167 // int x1 = xmin + d;
169 // int x2 = xmin + d + ymax - ymin;
171 // if(x1 < xmin) { y1 = y1 + (xmin - x1); x1 = xmin; }
172 // if(x2 > xmax) { y2 = y2 - (x2 - xmax); x2 = xmax; }
173 // image->draw_line(1, r, g, b, x1, y1, x2, y2);
177 image->draw_line(2, r, g, b, xmin, ymin, xmax, ymin);
178 image->draw_line(2, r, g, b, xmax, ymin, xmax, ymax);
179 image->draw_line(2, r, g, b, xmax, ymax, xmin, ymax);
180 image->draw_line(2, r, g, b, xmin, ymax, xmin, ymin);
183 for(int d = ymin - ymax; d <= xmax - xmin; d += delta) {
186 int x2 = xmin + d + ymax - ymin;
188 if(x1 < xmin) { y1 = y1 + (xmin - x1); x1 = xmin; }
189 if(x2 > xmax) { y2 = y2 - (x2 - xmax); x2 = xmax; }
190 image->draw_line(1, r, g, b, x1, y1, x2, y2);
197 void PiReferential::draw_edge_and_scale(RGBImage *image,
198 int registration_mode, Rectangle *window,
199 int _tag, int _edge_scale) {
200 const int ref_radius = 10;
204 switch(registration_mode) {
206 case PiReferential::RM_HEAD:
207 r = 0; g = 255; b = 0;
210 case PiReferential::RM_HEAD_NO_POLARITY:
211 r = 128; g = 255; b = 128;
214 case PiReferential::RM_BELLY:
215 r = 64; g = 0; b = 255;
218 case PiReferential::RM_BELLY_NO_POLARITY:
219 r = 192; g = 128; b = 255;
222 case PiReferential::RM_HEAD_BELLY_EDGES:
224 case PiReferential::RM_HEAD_BELLY:
225 r = 255; g = 0; b = 0;
228 case PiReferential::RM_BODY_EDGES:
230 case PiReferential::RM_BODY:
231 r = 0; g = 128; b = 255;
235 cerr << "INCONSISTENCY" << endl;
239 scalar_t xc = (window->xmin + window->xmax)/2;
240 scalar_t yc = (window->ymin + window->ymax)/2;
241 int radius = ref_radius * (1 << _edge_scale);
243 image->draw_ellipse(1, r, g, b, xc, yc, radius, radius, 0);
245 if(_tag >= RichImage::first_edge_tag && _tag < RichImage::first_edge_tag + RichImage::nb_edge_tags) {
249 switch(_tag - RichImage::first_edge_tag) {
286 scalar_t l = sqrt(dx * dx + dy * dy);
293 image->draw_ellipse(1, r, g, b, xc, yc, radius + delta, radius + delta, 0);
296 for(scalar_t u = 0; u <= radius; u += 0.1) {
297 scalar_t s = sqrt(radius * radius - (u * u * l * l))/l;
298 image->draw_line(2, r, g, b,
299 int(xc + u * dx - s * dy), int(yc + u * dy + s * dx),
300 int(xc + u * dx + s * dy), int(yc + u * dy - s * dx));
303 // for(int y = yc - radius; y <= yc + radius; y++) {
304 // for(int x = xc - radius; x <= xc + radius; x++) {
305 // if(x >= 0 && x < image->width() && y >= 0 && y < image->height() &&
306 // (x - xc) * dx + (y - yc) * dy >= 0) {
307 // image->draw_point(r, g, b, x, y);
314 else if(_tag == RichImage::variance_tag) {
315 image->draw_ellipse(1, r, g, b, xc, yc, 8, 8, 0);
318 // else if(_tag >= RichImage::first_gray_tag && _tag < RichImage::first_gray_tag + RichImage::nb_gray_tags) {
322 PiReferential::PiReferential(PoseCell *cell) {
323 scalar_t head_radius = sqrt(scalar_t(cell->_head_radius.min * cell->_head_radius.max));
325 _common_scale = global.scale_to_discrete_log_scale(head_radius / global.min_head_radius);
327 scalar_t discrete_scale_ratio = global.discrete_log_scale_to_scale(_common_scale);
329 //////////////////////////////////////////////////////////////////////
330 // Locations and scales
334 _head_xc = cell->_head_xc.middle() * discrete_scale_ratio;
335 _head_yc = cell->_head_yc.middle() * discrete_scale_ratio;
336 _head_radius = cell->_head_radius.middle() * discrete_scale_ratio;
337 _head_window_scaling = _head_radius * 2.0;
341 _body_xc = cell->_belly_xc.middle() * discrete_scale_ratio;
342 _body_yc = cell->_belly_yc.middle() * discrete_scale_ratio;
343 _body_window_scaling = sqrt(_body_radius_1 * _body_radius_2);
345 if((_head_xc - _body_xc) * cos(_body_tilt) + (_head_yc - _body_yc) * sin(_body_tilt) > 0) {
351 const scalar_t belly_frame_factor = 2.0;
353 _belly_xc = _body_xc;
354 _belly_yc = _body_yc;
355 _belly_window_scaling = _head_window_scaling * belly_frame_factor;
357 // Head-belly location
359 _head_belly_xc = (_head_xc + _body_xc) * 0.5;
360 _head_belly_yc = (_head_yc + _body_yc) * 0.5;
362 //////////////////////////////////////////////////////////////////////
365 if(_body_xc >= _head_xc) {
366 _horizontal_polarity = 1;
368 _horizontal_polarity = -1;
373 if(_horizontal_polarity < 0) {
374 _head_ux = _head_radius * 2.0;
377 _head_ux = - _head_radius * 2.0;
382 _head_vy = - _head_radius * 2.0;
384 _head_ux_nopolarity = _head_radius * 2.0;
385 _head_uy_nopolarity = 0;
386 _head_vx_nopolarity = 0;
387 _head_vy_nopolarity = - _head_radius * 2.0;
391 _belly_ux = _head_ux * belly_frame_factor;
392 _belly_uy = _head_uy * belly_frame_factor;
393 _belly_vx = _head_vx * belly_frame_factor;
394 _belly_vy = _head_vy * belly_frame_factor;
396 _belly_ux_nopolarity = _head_ux_nopolarity * belly_frame_factor;
397 _belly_uy_nopolarity = _head_uy_nopolarity * belly_frame_factor;
398 _belly_vx_nopolarity = _head_vx_nopolarity * belly_frame_factor;
399 _belly_vy_nopolarity = _head_vy_nopolarity * belly_frame_factor;
403 _head_belly_ux = 2 * (_head_xc - _head_belly_xc);
404 _head_belly_uy = 2 * (_head_yc - _head_belly_yc);
406 if(_horizontal_polarity < 0) {
407 _head_belly_vx = _head_belly_uy;
408 _head_belly_vy = - _head_belly_ux;
410 _head_belly_vx = - _head_belly_uy;
411 _head_belly_vy = _head_belly_ux;
414 scalar_t l = sqrt(_head_belly_vx * _head_belly_vx + _head_belly_vy * _head_belly_vy);
416 _head_belly_vx = _head_belly_vx/l * _head_radius * 2;
417 _head_belly_vy = _head_belly_vy/l * _head_radius * 2;
418 _head_belly_edge_shift = int(floor(- RichImage::nb_edge_tags * atan2(_head_belly_ux, _head_belly_uy) / (2 * M_PI) + 0.5));
419 _head_belly_edge_shift = (RichImage::nb_edge_tags + _head_belly_edge_shift) % RichImage::nb_edge_tags;
423 _body_ux = cos(_body_tilt) * _body_radius_1 * 2.0;
424 _body_uy = sin(_body_tilt) * _body_radius_1 * 2.0;
425 _body_vx = - sin(_body_tilt) * _body_radius_2 * 2.0;
426 _body_vy = cos(_body_tilt) * _body_radius_2 * 2.0;
428 _body_edge_shift = int(floor(RichImage::nb_edge_tags * _body_tilt / (2 * M_PI) + 0.5));
429 _body_edge_shift = (RichImage::nb_edge_tags + _body_edge_shift) % RichImage::nb_edge_tags;
432 int PiReferential::common_scale() {
433 return _common_scale;
436 void PiReferential::register_rectangle(int registration_mode,
439 scalar_t alpha, beta , xc, yc, w, h;
441 alpha = (original->xmin + original->xmax) * 0.5;
442 beta = (original->ymin + original->ymax) * 0.5;
444 switch(registration_mode) {
448 xc = _head_xc + alpha * _head_ux + beta * _head_vx;
449 yc = _head_yc + alpha * _head_uy + beta * _head_vy;
450 w = (original->xmax - original->xmin) * _head_window_scaling;
451 h = (original->ymax - original->ymin) * _head_window_scaling;
455 case RM_HEAD_NO_POLARITY:
457 xc = _head_xc + alpha * _head_ux_nopolarity + beta * _head_vx_nopolarity;
458 yc = _head_yc + alpha * _head_uy_nopolarity + beta * _head_vy_nopolarity;
459 w = (original->xmax - original->xmin) * _head_window_scaling;
460 h = (original->ymax - original->ymin) * _head_window_scaling;
466 xc = _belly_xc + alpha * _belly_ux + beta * _belly_vx;
467 yc = _belly_yc + alpha * _belly_uy + beta * _belly_vy;
468 w = (original->xmax - original->xmin) * _belly_window_scaling;
469 h = (original->ymax - original->ymin) * _belly_window_scaling;
473 case RM_BELLY_NO_POLARITY:
475 xc = _belly_xc + alpha * _belly_ux_nopolarity + beta * _belly_vx_nopolarity;
476 yc = _belly_yc + alpha * _belly_uy_nopolarity + beta * _belly_vy_nopolarity;
477 w = (original->xmax - original->xmin) * _belly_window_scaling;
478 h = (original->ymax - original->ymin) * _belly_window_scaling;
483 case RM_HEAD_BELLY_EDGES:
485 xc = _head_belly_xc + alpha * _head_belly_ux + beta * _head_belly_vx;
486 yc = _head_belly_yc + alpha * _head_belly_uy + beta * _head_belly_vy;
487 w = (original->xmax - original->xmin) * _head_window_scaling;
488 h = (original->ymax - original->ymin) * _head_window_scaling;
495 xc = _body_xc + alpha * _body_ux + beta * _body_vx;
496 yc = _body_yc + alpha * _body_uy + beta * _body_vy;
497 w = (original->xmax - original->xmin) * _body_window_scaling;
498 h = (original->ymax - original->ymin) * _body_window_scaling;
503 cerr << "Undefined registration mode." << endl;
507 result->xmin = xc - 0.5 * w;
508 result->ymin = yc - 0.5 * h;
509 result->xmax = xc + 0.5 * w;
510 result->ymax = yc + 0.5 * h;
512 ASSERT(result->xmin < result->xmax && result->ymin < result->ymax);
515 int PiReferential::register_edge(int registration_mode, int edge_type) {
517 if(edge_type >= RichImage::first_edge_tag &&
518 edge_type < RichImage::first_edge_tag + RichImage::nb_edge_tags) {
520 int e = edge_type - RichImage::first_edge_tag;
522 switch(registration_mode) {
523 case PiReferential::RM_HEAD_NO_POLARITY:
524 case PiReferential::RM_BELLY_NO_POLARITY:
527 case PiReferential::RM_HEAD:
528 case PiReferential::RM_BELLY:
529 case PiReferential::RM_HEAD_BELLY:
530 case PiReferential::RM_BODY:
531 if(_horizontal_polarity < 0) {
532 e = (RichImage::nb_edge_tags - e) % RichImage::nb_edge_tags;
536 case PiReferential::RM_HEAD_BELLY_EDGES:
537 if(_horizontal_polarity < 0) {
538 e = (RichImage::nb_edge_tags - e) % RichImage::nb_edge_tags;
540 e += _head_belly_edge_shift;
543 case PiReferential::RM_BODY_EDGES:
544 if(_horizontal_polarity < 0) {
545 e = (RichImage::nb_edge_tags - e) % RichImage::nb_edge_tags;
547 e += _body_edge_shift;
551 cerr << "INCONSISTENCY" << endl;
555 e = e % RichImage::nb_edge_tags;
557 return RichImage::first_edge_tag + e;
561 else return edge_type;
564 void PiReferential::draw(RGBImage *image, int level) {
565 int x1, y1, x2, y2, x3, y3, x4, y4;
569 // Draw the RM_BODY reference frame
571 x1 = int(_body_xc + _body_ux + _body_vx);
572 y1 = int(_body_yc + _body_uy + _body_vy);
573 x2 = int(_body_xc - _body_ux + _body_vx);
574 y2 = int(_body_yc - _body_uy + _body_vy);
575 x3 = int(_body_xc - _body_ux - _body_vx);
576 y3 = int(_body_yc - _body_uy - _body_vy);
577 x4 = int(_body_xc + _body_ux - _body_vx);
578 y4 = int(_body_yc + _body_uy - _body_vy);
580 draw_frame(image, RM_BODY, x1, y1, x2, y2, x3, y3, x4, y4);
585 // Draw the RM_BELLY reference frame
587 x1 = int(_belly_xc + _belly_ux + _belly_vx);
588 y1 = int(_belly_yc + _belly_uy + _belly_vy);
589 x2 = int(_belly_xc - _belly_ux + _belly_vx);
590 y2 = int(_belly_yc - _belly_uy + _belly_vy);
591 x3 = int(_belly_xc - _belly_ux - _belly_vx);
592 y3 = int(_belly_yc - _belly_uy - _belly_vy);
593 x4 = int(_belly_xc + _belly_ux - _belly_vx);
594 y4 = int(_belly_yc + _belly_uy - _belly_vy);
596 draw_frame(image, RM_BELLY, x1, y1, x2, y2, x3, y3, x4, y4);
598 // Draw the RM_HEAD_BELLY reference frame
600 x1 = int(_head_belly_xc + _head_belly_ux + _head_belly_vx);
601 y1 = int(_head_belly_yc + _head_belly_uy + _head_belly_vy);
602 x2 = int(_head_belly_xc - _head_belly_ux + _head_belly_vx);
603 y2 = int(_head_belly_yc - _head_belly_uy + _head_belly_vy);
604 x3 = int(_head_belly_xc - _head_belly_ux - _head_belly_vx);
605 y3 = int(_head_belly_yc - _head_belly_uy - _head_belly_vy);
606 x4 = int(_head_belly_xc + _head_belly_ux - _head_belly_vx);
607 y4 = int(_head_belly_yc + _head_belly_uy - _head_belly_vy);
609 draw_frame(image, RM_HEAD_BELLY, x1, y1, x2, y2, x3, y3, x4, y4);
612 // Draw the RM_HEAD reference frame
614 x1 = int(_head_xc + _head_ux + _head_vx);
615 y1 = int(_head_yc + _head_uy + _head_vy);
616 x2 = int(_head_xc - _head_ux + _head_vx);
617 y2 = int(_head_yc - _head_uy + _head_vy);
618 x3 = int(_head_xc - _head_ux - _head_vx);
619 y3 = int(_head_yc - _head_uy - _head_vy);
620 x4 = int(_head_xc + _head_ux - _head_vx);
621 y4 = int(_head_yc + _head_uy - _head_vy);
623 draw_frame(image, RM_HEAD, x1, y1, x2, y2, x3, y3, x4, y4);
626 void PiReferential::print_registration_mode(ostream *out, int registration_mode) {
627 switch(registration_mode) {
631 case RM_HEAD_NO_POLARITY:
632 (*out) << "RM_HEAD_NO_POLARITY";
635 (*out) << "RM_BELLY";
637 case RM_BELLY_NO_POLARITY:
638 (*out) << "RM_BELLY_NO_POLARITY";
641 (*out) << "RM_HEAD_BELLY";
643 case RM_HEAD_BELLY_EDGES:
644 (*out) << "RM_HEAD_BELLY_EDGES";
650 (*out) << "RM_BODY_EDGES";