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 "pose_cell_hierarchy.h"
22 PoseCellHierarchy::PoseCellHierarchy() {
26 PoseCellHierarchy::PoseCellHierarchy(LabelledImagePool *train_pool) {
27 _nb_levels = global.nb_levels;
28 _min_head_radius = global.min_head_radius;
29 _max_head_radius = global.max_head_radius;
30 _root_cell_nb_xy_per_scale = global.root_cell_nb_xy_per_scale;
33 int nb_total_targets = 0;
34 for(int i = 0; i < train_pool->nb_images(); i++) {
35 image = train_pool->grab_image(i);
36 // We are going to symmetrize
37 nb_total_targets += 2 * image->nb_targets();
38 train_pool->release_image(i);
41 RelativeBodyPoseCell targets[nb_total_targets];
44 for(int i = 0; i < train_pool->nb_images(); i++) {
45 image = train_pool->grab_image(i);
48 add_root_cells(image, &cell_set);
50 for(int t = 0; t < image->nb_targets(); t++) {
51 Pose pose = *image->get_target_pose(t);
54 cell_set.get_containing_cell(&pose)->get_centroid(&coarse);
56 targets[u]._body_xc.set((pose._belly_xc - coarse._head_xc) / coarse._head_radius);
57 targets[u]._body_yc.set((pose._belly_yc - coarse._head_yc) / coarse._head_radius);
60 pose.horizontal_flip(image->width());
62 cell_set.get_containing_cell(&pose)->get_centroid(&coarse);
64 targets[u]._body_xc.set((pose._belly_xc - coarse._head_xc) / coarse._head_radius);
65 targets[u]._body_yc.set((pose._belly_yc - coarse._head_yc) / coarse._head_radius);
69 train_pool->release_image(i);
72 scalar_t fattening = 1.1;
74 Interval body_rxc, body_ryc;
76 body_rxc.set(&targets[0]._body_xc);
77 body_ryc.set(&targets[0]._body_yc);
79 for(int t = 0; t < nb_total_targets; t++) {
80 body_rxc.swallow(&targets[t]._body_xc);
81 body_ryc.swallow(&targets[t]._body_yc);
84 body_rxc.min *= fattening;
85 body_rxc.max *= fattening;
86 body_ryc.min *= fattening;
87 body_ryc.max *= fattening;
89 scalar_t body_rxc_min = body_resolution * floor(body_rxc.min / body_resolution);
90 int nb_body_rxc = int(ceil((body_rxc.max - body_rxc_min) / body_resolution));
92 scalar_t body_ryc_min = body_resolution * floor(body_ryc.min / body_resolution);
93 int nb_body_ryc = int(ceil((body_ryc.max - body_ryc_min) / body_resolution));
95 (*global.log_stream) << "body_rxc = " << body_rxc << endl
96 << "body_rxc_min = " << body_rxc_min << endl
97 << "body_rxc_min + nb_body_rxc * body_resolution = " << body_rxc_min + nb_body_rxc * body_resolution << endl
99 << "body_ryc = " << body_ryc << endl
100 << "body_ryc_min = " << body_ryc_min << endl
101 << "body_ryc_min + nb_body_ryc * body_resolution = " << body_ryc_min + nb_body_ryc * body_resolution << endl;
103 int used[nb_body_rxc * nb_body_rxc];
105 for(int k = 0; k < nb_body_rxc * nb_body_ryc; k++) {
109 // An ugly way to compute the convexe enveloppe
111 for(scalar_t alpha = 0; alpha < M_PI * 2; alpha += (2 * M_PI) / 100) {
112 scalar_t vx = cos(alpha), vy = sin(alpha);
115 for(int t = 0; t < nb_total_targets; t++) {
116 rho = min(rho, vx * targets[t]._body_xc.middle() + vy * targets[t]._body_yc.middle());
121 for(int j = 0; j < nb_body_ryc; j++) {
122 for(int i = 0; i < nb_body_rxc; i++) {
124 vx * (scalar_t(i + 0) * body_resolution + body_rxc_min) +
125 vy * (scalar_t(j + 0) * body_resolution + body_ryc_min) < rho
127 vx * (scalar_t(i + 1) * body_resolution + body_rxc_min) +
128 vy * (scalar_t(j + 0) * body_resolution + body_ryc_min) < rho
130 vx * (scalar_t(i + 0) * body_resolution + body_rxc_min) +
131 vy * (scalar_t(j + 1) * body_resolution + body_ryc_min) < rho
133 vx * (scalar_t(i + 1) * body_resolution + body_rxc_min) +
134 vy * (scalar_t(j + 1) * body_resolution + body_ryc_min) < rho
136 used[i + j * nb_body_rxc] = 0;
143 for(int j = 0; j < nb_body_ryc; j++) {
144 for(int i = 0; i < nb_body_rxc; i++) {
145 if(used[i + nb_body_rxc * j]) {
151 _body_cells = new RelativeBodyPoseCell[_nb_body_cells];
153 for(int j = 0; j < nb_body_ryc; j++) {
154 for(int i = 0; i < nb_body_rxc; i++) {
155 if(used[i + nb_body_rxc * j]) {
156 if(sq(scalar_t(i) * body_resolution + body_resolution/2 + body_rxc_min) +
157 sq(scalar_t(j) * body_resolution + body_resolution/2 + body_ryc_min) <= 1) {
158 (*global.log_stream) << "*";
160 (*global.log_stream) << "X";
163 (*global.log_stream) << ".";
166 (*global.log_stream) << endl;
170 for(int j = 0; j < nb_body_ryc; j++) {
171 for(int i = 0; i < nb_body_rxc; i++) {
173 if(used[i + nb_body_rxc * j]) {
175 RelativeBodyPoseCell mother;
177 scalar_t x = scalar_t(i) * body_resolution + body_rxc_min;
178 scalar_t y = scalar_t(j) * body_resolution + body_ryc_min;
180 mother._body_xc.set(x, x + body_resolution);
181 mother._body_yc.set(y, y + body_resolution);
183 _body_cells[k++] = mother;
188 (*global.log_stream) << _nb_body_cells << " body cells." << endl;
191 PoseCellHierarchy::~PoseCellHierarchy() {
192 delete[] _body_cells;
195 int PoseCellHierarchy::nb_levels() {
199 void PoseCellHierarchy::get_containing_cell(Image *image, int level,
200 Pose *pose, PoseCell *result_cell) {
201 PoseCellSet cell_set;
203 for(int l = 0; l < level + 1; l++) {
204 cell_set.erase_content();
206 add_root_cells(image, &cell_set);
208 add_subcells(l, result_cell, &cell_set);
211 *result_cell = *(cell_set.get_containing_cell(pose));
215 void PoseCellHierarchy::add_root_cells(Image *image, PoseCellSet *cell_set) {
217 const int nb_scales = int((log(_max_head_radius) - log(_min_head_radius)) / log(2) *
218 global.nb_scales_per_power_of_two);
220 scalar_t alpha = log(_min_head_radius);
221 scalar_t beta = log(2) / scalar_t(global.nb_scales_per_power_of_two);
223 for(int s = 0; s < nb_scales; s++) {
224 scalar_t cell_xy_size = exp(alpha + scalar_t(s) * beta) / global.root_cell_nb_xy_per_scale;
226 cell._head_radius.min = exp(alpha + scalar_t(s) * beta);
227 cell._head_radius.max = exp(alpha + scalar_t(s+1) * beta);
228 cell._head_tilt.min = -M_PI;
229 cell._head_tilt.max = M_PI;
230 for(scalar_t y = 0; y < image->height(); y += cell_xy_size)
231 for(scalar_t x = 0; x < image->width(); x += cell_xy_size) {
232 cell._head_xc.min = x;
233 cell._head_xc.max = x + cell_xy_size;
234 cell._head_yc.min = y;
235 cell._head_yc.max = y + cell_xy_size;
236 cell._belly_xc.min = cell._head_xc.min - pseudo_infty;
237 cell._belly_xc.max = cell._head_xc.max + pseudo_infty;
238 cell._belly_yc.min = cell._head_yc.min - pseudo_infty;
239 cell._belly_yc.max = cell._head_yc.max + pseudo_infty;
240 cell_set->add_cell(&cell);
245 void PoseCellHierarchy::add_subcells(int level, PoseCell *root,
246 PoseCellSet *cell_set) {
252 // Here we split the body-center coordinate cell part
253 PoseCell cell = *root;
254 scalar_t r = sqrt(cell._head_radius.min * cell._head_radius.max);
255 scalar_t x = (cell._head_xc.min + cell._head_xc.max) / 2.0;
256 scalar_t y = (cell._head_yc.min + cell._head_yc.max) / 2.0;
257 for(int k = 0; k < _nb_body_cells; k++) {
258 cell._belly_xc.min = (_body_cells[k]._body_xc.min * r) + x;
259 cell._belly_xc.max = (_body_cells[k]._body_xc.max * r) + x;
260 cell._belly_yc.min = (_body_cells[k]._body_yc.min * r) + y;
261 cell._belly_yc.max = (_body_cells[k]._body_yc.max * r) + y;
262 cell_set->add_cell(&cell);
269 cerr << "Inconsistent level in PoseCellHierarchy::add_subcells" << endl;
277 int PoseCellHierarchy::nb_incompatible_poses(LabelledImagePool *pool) {
278 PoseCell target_cell;
279 PoseCellSet cell_set;
280 LabelledImage *image;
284 for(int i = 0; i < pool->nb_images(); i++) {
285 image = pool->grab_image(i);
287 for(int t = 0; t < image->nb_targets(); t++) {
288 cell_set.erase_content();
290 int error_level = -1;
292 for(int l = 0; error_level < 0 && l < _nb_levels; l++) {
293 cell_set.erase_content();
296 add_root_cells(image, &cell_set);
298 add_subcells(l, &target_cell, &cell_set);
301 int nb_compliant = 0;
303 for(int c = 0; c < cell_set.nb_cells(); c++) {
304 if(cell_set.get_cell(c)->contains(image->get_target_pose(t))) {
305 target_cell = *(cell_set.get_cell(c));
310 if(nb_compliant != 1) {
315 if(error_level >= 0) {
320 pool->release_image(i);
326 void PoseCellHierarchy::write(ostream *os) {
327 write_var(os, &_min_head_radius);
328 write_var(os, &_max_head_radius);
329 write_var(os, &_root_cell_nb_xy_per_scale);
330 write_var(os, &_nb_body_cells);
331 for(int k = 0; k < _nb_body_cells; k++)
332 write_var(os, &_body_cells[k]);
335 void PoseCellHierarchy::read(istream *is) {
336 delete[] _body_cells;
337 read_var(is, &_min_head_radius);
338 read_var(is, &_max_head_radius);
339 read_var(is, &_root_cell_nb_xy_per_scale);
340 read_var(is, &_nb_body_cells);
341 delete[] _body_cells;
342 _body_cells = new RelativeBodyPoseCell[_nb_body_cells];
343 for(int k = 0; k < _nb_body_cells; k++) {
344 read_var(is, &_body_cells[k]);