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 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);
80 void PiReferential::draw_window(RGBImage *image,
81 int registration_mode, Rectangle *window,
85 switch(registration_mode) {
87 case PiReferential::RM_HEAD:
88 r = 0; g = 255; b = 0;
91 case PiReferential::RM_HEAD_NO_POLARITY:
92 r = 128; g = 255; b = 128;
95 case PiReferential::RM_BELLY:
96 r = 64; g = 0; b = 255;
99 case PiReferential::RM_BELLY_NO_POLARITY:
100 r = 192; g = 128; b = 255;
103 case PiReferential::RM_HEAD_BELLY:
104 case PiReferential::RM_HEAD_BELLY_EDGES:
105 r = 255; g = 0; b = 0;
109 cerr << "INCONSISTENCY" << endl;
113 int xmin = int(window->xmin);
114 int ymin = int(window->ymin);
115 int xmax = int(window->xmax);
116 int ymax = int(window->ymax);
118 if(global.pictures_for_article) {
119 r = 255; g = 255; b = 255;
120 image->draw_line(6, r, g, b, xmin, ymin, xmax, ymin);
121 image->draw_line(6, r, g, b, xmax, ymin, xmax, ymax);
122 image->draw_line(6, r, g, b, xmax, ymax, xmin, ymax);
123 image->draw_line(6, r, g, b, xmin, ymax, xmin, ymin);
126 image->draw_line(2, r, g, b, xmin, ymin, xmax, ymin);
127 image->draw_line(2, r, g, b, xmax, ymin, xmax, ymax);
128 image->draw_line(2, r, g, b, xmax, ymax, xmin, ymax);
129 image->draw_line(2, r, g, b, xmin, ymax, xmin, ymin);
132 image->draw_line(2, r, g, b, xmin, ymin, xmax, ymin);
133 image->draw_line(2, r, g, b, xmax, ymin, xmax, ymax);
134 image->draw_line(2, r, g, b, xmax, ymax, xmin, ymax);
135 image->draw_line(2, r, g, b, xmin, ymax, xmin, ymin);
138 for(int d = ymin - ymax; d <= xmax - xmin; d += delta) {
141 int x2 = xmin + d + ymax - ymin;
143 if(x1 < xmin) { y1 = y1 + (xmin - x1); x1 = xmin; }
144 if(x2 > xmax) { y2 = y2 - (x2 - xmax); x2 = xmax; }
145 image->draw_line(1, r, g, b, x1, y1, x2, y2);
152 PiReferential::PiReferential(PoseCell *cell) {
153 scalar_t head_radius = sqrt(scalar_t(cell->_head_radius.min * cell->_head_radius.max));
155 _common_scale = global.scale_to_discrete_log_scale(head_radius / global.min_head_radius);
157 scalar_t discrete_scale_ratio = global.discrete_log_scale_to_scale(_common_scale);
159 //////////////////////////////////////////////////////////////////////
160 // Locations and scales
164 _head_xc = cell->_head_xc.middle() * discrete_scale_ratio;
165 _head_yc = cell->_head_yc.middle() * discrete_scale_ratio;
166 _head_radius = cell->_head_radius.middle() * discrete_scale_ratio;
167 _head_window_scaling = _head_radius * 2.0;
171 // **********************************************************************
172 // Useless code, but necessary to keep the exact same results with
173 // g++ 4.1 and -O3 options on reference experiments.
174 _body_xc = cell->_belly_xc.middle() * discrete_scale_ratio;
175 _body_yc = cell->_belly_yc.middle() * discrete_scale_ratio;
177 if((_head_xc - _body_xc) * cos(_body_tilt) + (_head_yc - _body_yc) * sin(_body_tilt) > 0) {
180 // **********************************************************************
184 const scalar_t belly_frame_factor = 2.0;
186 _belly_xc = cell->_belly_xc.middle() * discrete_scale_ratio;
187 _belly_yc = cell->_belly_yc.middle() * discrete_scale_ratio;
188 _belly_window_scaling = _head_window_scaling * belly_frame_factor;
190 // Head-belly location
192 _head_belly_xc = (_head_xc + _belly_xc) * 0.5;
193 _head_belly_yc = (_head_yc + _belly_yc) * 0.5;
195 //////////////////////////////////////////////////////////////////////
198 if(_body_xc >= _head_xc) {
199 // if(_belly_xc >= _head_xc) {
200 _horizontal_polarity = 1;
202 _horizontal_polarity = -1;
207 if(_horizontal_polarity < 0) {
208 _head_ux = _head_radius * 2.0;
211 _head_ux = - _head_radius * 2.0;
216 _head_vy = - _head_radius * 2.0;
218 _head_ux_nopolarity = _head_radius * 2.0;
219 _head_uy_nopolarity = 0;
220 _head_vx_nopolarity = 0;
221 _head_vy_nopolarity = - _head_radius * 2.0;
225 _belly_ux = _head_ux * belly_frame_factor;
226 _belly_uy = _head_uy * belly_frame_factor;
227 _belly_vx = _head_vx * belly_frame_factor;
228 _belly_vy = _head_vy * belly_frame_factor;
230 _belly_ux_nopolarity = _head_ux_nopolarity * belly_frame_factor;
231 _belly_uy_nopolarity = _head_uy_nopolarity * belly_frame_factor;
232 _belly_vx_nopolarity = _head_vx_nopolarity * belly_frame_factor;
233 _belly_vy_nopolarity = _head_vy_nopolarity * belly_frame_factor;
237 _head_belly_ux = 2 * (_head_xc - _head_belly_xc);
238 _head_belly_uy = 2 * (_head_yc - _head_belly_yc);
240 if(_horizontal_polarity < 0) {
241 _head_belly_vx = _head_belly_uy;
242 _head_belly_vy = - _head_belly_ux;
244 _head_belly_vx = - _head_belly_uy;
245 _head_belly_vy = _head_belly_ux;
248 scalar_t l = sqrt(_head_belly_vx * _head_belly_vx + _head_belly_vy * _head_belly_vy);
250 _head_belly_vx = (_head_belly_vx / l) * _head_radius * 2;
251 _head_belly_vy = (_head_belly_vy / l) * _head_radius * 2;
252 _head_belly_edge_shift = int(floor(- RichImage::nb_edge_tags * atan2(_head_belly_ux, _head_belly_uy) / (2 * M_PI) + 0.5));
253 _head_belly_edge_shift = (RichImage::nb_edge_tags + _head_belly_edge_shift) % RichImage::nb_edge_tags;
256 int PiReferential::common_scale() {
257 return _common_scale;
260 void PiReferential::register_rectangle(int registration_mode,
263 scalar_t alpha, beta , xc, yc, w, h;
265 alpha = (original->xmin + original->xmax) * 0.5;
266 beta = (original->ymin + original->ymax) * 0.5;
268 switch(registration_mode) {
272 xc = _head_xc + alpha * _head_ux + beta * _head_vx;
273 yc = _head_yc + alpha * _head_uy + beta * _head_vy;
274 w = (original->xmax - original->xmin) * _head_window_scaling;
275 h = (original->ymax - original->ymin) * _head_window_scaling;
279 case RM_HEAD_NO_POLARITY:
281 xc = _head_xc + alpha * _head_ux_nopolarity + beta * _head_vx_nopolarity;
282 yc = _head_yc + alpha * _head_uy_nopolarity + beta * _head_vy_nopolarity;
283 w = (original->xmax - original->xmin) * _head_window_scaling;
284 h = (original->ymax - original->ymin) * _head_window_scaling;
290 xc = _belly_xc + alpha * _belly_ux + beta * _belly_vx;
291 yc = _belly_yc + alpha * _belly_uy + beta * _belly_vy;
292 w = (original->xmax - original->xmin) * _belly_window_scaling;
293 h = (original->ymax - original->ymin) * _belly_window_scaling;
297 case RM_BELLY_NO_POLARITY:
299 xc = _belly_xc + alpha * _belly_ux_nopolarity + beta * _belly_vx_nopolarity;
300 yc = _belly_yc + alpha * _belly_uy_nopolarity + beta * _belly_vy_nopolarity;
301 w = (original->xmax - original->xmin) * _belly_window_scaling;
302 h = (original->ymax - original->ymin) * _belly_window_scaling;
307 case RM_HEAD_BELLY_EDGES:
309 xc = _head_belly_xc + alpha * _head_belly_ux + beta * _head_belly_vx;
310 yc = _head_belly_yc + alpha * _head_belly_uy + beta * _head_belly_vy;
311 w = (original->xmax - original->xmin) * _head_window_scaling;
312 h = (original->ymax - original->ymin) * _head_window_scaling;
317 cerr << "Undefined registration mode." << endl;
321 result->xmin = xc - 0.5 * w;
322 result->ymin = yc - 0.5 * h;
323 result->xmax = xc + 0.5 * w;
324 result->ymax = yc + 0.5 * h;
326 ASSERT(result->xmin < result->xmax && result->ymin < result->ymax);
329 int PiReferential::register_edge(int registration_mode, int edge_type) {
331 if(edge_type >= RichImage::first_edge_tag &&
332 edge_type < RichImage::first_edge_tag + RichImage::nb_edge_tags) {
334 int e = edge_type - RichImage::first_edge_tag;
336 switch(registration_mode) {
337 case PiReferential::RM_HEAD_NO_POLARITY:
338 case PiReferential::RM_BELLY_NO_POLARITY:
341 case PiReferential::RM_HEAD:
342 case PiReferential::RM_BELLY:
343 case PiReferential::RM_HEAD_BELLY:
344 if(_horizontal_polarity < 0) {
345 e = (RichImage::nb_edge_tags - e) % RichImage::nb_edge_tags;
349 case PiReferential::RM_HEAD_BELLY_EDGES:
350 if(_horizontal_polarity < 0) {
351 e = (RichImage::nb_edge_tags - e) % RichImage::nb_edge_tags;
353 e += _head_belly_edge_shift;
357 cerr << "INCONSISTENCY" << endl;
361 e = e % RichImage::nb_edge_tags;
363 return RichImage::first_edge_tag + e;
367 else return edge_type;
370 void PiReferential::draw(RGBImage *image, int level) {
371 int x1, y1, x2, y2, x3, y3, x4, y4;
375 // Draw the RM_BELLY reference frame
377 x1 = int(_belly_xc + _belly_ux + _belly_vx);
378 y1 = int(_belly_yc + _belly_uy + _belly_vy);
379 x2 = int(_belly_xc - _belly_ux + _belly_vx);
380 y2 = int(_belly_yc - _belly_uy + _belly_vy);
381 x3 = int(_belly_xc - _belly_ux - _belly_vx);
382 y3 = int(_belly_yc - _belly_uy - _belly_vy);
383 x4 = int(_belly_xc + _belly_ux - _belly_vx);
384 y4 = int(_belly_yc + _belly_uy - _belly_vy);
386 draw_frame(image, RM_BELLY, x1, y1, x2, y2, x3, y3, x4, y4);
388 // Draw the RM_HEAD_BELLY reference frame
390 x1 = int(_head_belly_xc + _head_belly_ux + _head_belly_vx);
391 y1 = int(_head_belly_yc + _head_belly_uy + _head_belly_vy);
392 x2 = int(_head_belly_xc - _head_belly_ux + _head_belly_vx);
393 y2 = int(_head_belly_yc - _head_belly_uy + _head_belly_vy);
394 x3 = int(_head_belly_xc - _head_belly_ux - _head_belly_vx);
395 y3 = int(_head_belly_yc - _head_belly_uy - _head_belly_vy);
396 x4 = int(_head_belly_xc + _head_belly_ux - _head_belly_vx);
397 y4 = int(_head_belly_yc + _head_belly_uy - _head_belly_vy);
399 draw_frame(image, RM_HEAD_BELLY, x1, y1, x2, y2, x3, y3, x4, y4);
402 // Draw the RM_HEAD reference frame
404 x1 = int(_head_xc + _head_ux + _head_vx);
405 y1 = int(_head_yc + _head_uy + _head_vy);
406 x2 = int(_head_xc - _head_ux + _head_vx);
407 y2 = int(_head_yc - _head_uy + _head_vy);
408 x3 = int(_head_xc - _head_ux - _head_vx);
409 y3 = int(_head_yc - _head_uy - _head_vy);
410 x4 = int(_head_xc + _head_ux - _head_vx);
411 y4 = int(_head_yc + _head_uy - _head_vy);
413 draw_frame(image, RM_HEAD, x1, y1, x2, y2, x3, y3, x4, y4);
416 void PiReferential::print_registration_mode(ostream *out, int registration_mode) {
417 switch(registration_mode) {
421 case RM_HEAD_NO_POLARITY:
422 (*out) << "RM_HEAD_NO_POLARITY";
425 (*out) << "RM_BELLY";
427 case RM_BELLY_NO_POLARITY:
428 (*out) << "RM_BELLY_NO_POLARITY";
431 (*out) << "RM_HEAD_BELLY";
433 case RM_HEAD_BELLY_EDGES:
434 (*out) << "RM_HEAD_BELLY_EDGES";