X-Git-Url: https://fleuret.org/cgi-bin/gitweb/gitweb.cgi?a=blobdiff_plain;f=pose_cell_hierarchy.cc;fp=pose_cell_hierarchy.cc;h=0816f3a2819ce94a2e43bd71a8c2b1eaa4e7b7ec;hb=d922ad61d35e9a6996730bec24b16f8bf7bc426c;hp=0000000000000000000000000000000000000000;hpb=3bb118f5a9462d02ff7d99ef28ecc0d7e23529f9;p=folded-ctf.git
diff --git a/pose_cell_hierarchy.cc b/pose_cell_hierarchy.cc
new file mode 100644
index 0000000..0816f3a
--- /dev/null
+++ b/pose_cell_hierarchy.cc
@@ -0,0 +1,392 @@
+
+///////////////////////////////////////////////////////////////////////////
+// This program is free software: you can redistribute it and/or modify //
+// it under the terms of the version 3 of the GNU General Public License //
+// as published by the Free Software Foundation. //
+// //
+// This program is distributed in the hope that it will be useful, but //
+// WITHOUT ANY WARRANTY; without even the implied warranty of //
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU //
+// General Public License for more details. //
+// //
+// You should have received a copy of the GNU General Public License //
+// along with this program. If not, see . //
+// //
+// Written by Francois Fleuret, (C) IDIAP //
+// Contact for comments & bug reports //
+///////////////////////////////////////////////////////////////////////////
+
+#include "pose_cell_hierarchy.h"
+#include "gaussian.h"
+
+PoseCellHierarchy::PoseCellHierarchy() {
+ _body_cells = 0;
+}
+
+PoseCellHierarchy::PoseCellHierarchy(LabelledImagePool *train_pool) {
+ _nb_levels = global.nb_levels;
+ _min_head_radius = global.min_head_radius;
+ _max_head_radius = global.max_head_radius;
+ _root_cell_nb_xy_per_scale = global.root_cell_nb_xy_per_scale;
+
+ LabelledImage *image;
+ int nb_total_targets = 0;
+ for(int i = 0; i < train_pool->nb_images(); i++) {
+ image = train_pool->grab_image(i);
+ // We are going to symmetrize
+ nb_total_targets += 2 * image->nb_targets();
+ train_pool->release_image(i);
+ }
+
+ RelativeBodyPoseCell targets[nb_total_targets];
+
+ int u = 0;
+ for(int i = 0; i < train_pool->nb_images(); i++) {
+ image = train_pool->grab_image(i);
+
+ PoseCellSet cell_set;
+ add_root_cells(image, &cell_set);
+
+ for(int t = 0; t < image->nb_targets(); t++) {
+ Pose pose = *image->get_target_pose(t);
+ Pose coarse;
+
+ cell_set.get_containing_cell(&pose)->get_centroid(&coarse);
+
+ targets[u]._body_xc.set((pose._body_xc - coarse._head_xc) / coarse._head_radius);
+ targets[u]._body_yc.set((pose._body_yc - coarse._head_yc) / coarse._head_radius);
+ u++;
+
+ pose.horizontal_flip(image->width());
+
+ cell_set.get_containing_cell(&pose)->get_centroid(&coarse);
+
+ targets[u]._body_xc.set((pose._body_xc - coarse._head_xc) / coarse._head_radius);
+ targets[u]._body_yc.set((pose._body_yc - coarse._head_yc) / coarse._head_radius);
+ u++;
+ }
+
+ train_pool->release_image(i);
+ }
+
+ scalar_t fattening = 1.1;
+
+ Interval body_rxc, body_ryc;
+
+ body_rxc.set(&targets[0]._body_xc);
+ body_ryc.set(&targets[0]._body_yc);
+
+ for(int t = 0; t < nb_total_targets; t++) {
+ body_rxc.swallow(&targets[t]._body_xc);
+ body_ryc.swallow(&targets[t]._body_yc);
+ }
+
+ body_rxc.min *= fattening;
+ body_rxc.max *= fattening;
+ body_ryc.min *= fattening;
+ body_ryc.max *= fattening;
+
+ scalar_t body_rxc_min = body_resolution * floor(body_rxc.min / body_resolution);
+ int nb_body_rxc = int(ceil((body_rxc.max - body_rxc_min) / body_resolution));
+
+ scalar_t body_ryc_min = body_resolution * floor(body_ryc.min / body_resolution);
+ int nb_body_ryc = int(ceil((body_ryc.max - body_ryc_min) / body_resolution));
+
+ (*global.log_stream) << "body_rxc = " << body_rxc << endl
+ << "body_rxc_min = " << body_rxc_min << endl
+ << "body_rxc_min + nb_body_rxc * body_resolution = " << body_rxc_min + nb_body_rxc * body_resolution << endl
+ << endl
+ << "body_ryc = " << body_ryc << endl
+ << "body_ryc_min = " << body_ryc_min << endl
+ << "body_ryc_min + nb_body_ryc * body_resolution = " << body_ryc_min + nb_body_ryc * body_resolution << endl;
+
+ int used[nb_body_rxc * nb_body_rxc];
+
+ for(int k = 0; k < nb_body_rxc * nb_body_ryc; k++) {
+ used[k] = 1;
+ }
+
+ // An ugly way to compute the convexe enveloppe
+
+ for(scalar_t alpha = 0; alpha < M_PI * 2; alpha += (2 * M_PI) / 100) {
+ scalar_t vx = cos(alpha), vy = sin(alpha);
+ scalar_t rho = 0;
+
+ for(int t = 0; t < nb_total_targets; t++) {
+ rho = min(rho, vx * targets[t]._body_xc.middle() + vy * targets[t]._body_yc.middle());
+ }
+
+ rho *= fattening;
+
+ for(int j = 0; j < nb_body_ryc; j++) {
+ for(int i = 0; i < nb_body_rxc; i++) {
+ if(
+ vx * (scalar_t(i + 0) * body_resolution + body_rxc_min) +
+ vy * (scalar_t(j + 0) * body_resolution + body_ryc_min) < rho
+ &&
+ vx * (scalar_t(i + 1) * body_resolution + body_rxc_min) +
+ vy * (scalar_t(j + 0) * body_resolution + body_ryc_min) < rho
+ &&
+ vx * (scalar_t(i + 0) * body_resolution + body_rxc_min) +
+ vy * (scalar_t(j + 1) * body_resolution + body_ryc_min) < rho
+ &&
+ vx * (scalar_t(i + 1) * body_resolution + body_rxc_min) +
+ vy * (scalar_t(j + 1) * body_resolution + body_ryc_min) < rho
+ ) {
+ used[i + j * nb_body_rxc] = 0;
+ }
+ }
+ }
+ }
+
+ _nb_body_cells = 0;
+ for(int j = 0; j < nb_body_ryc; j++) {
+ for(int i = 0; i < nb_body_rxc; i++) {
+ if(used[i + nb_body_rxc * j]) {
+ _nb_body_cells++;
+ }
+ }
+ }
+
+ _body_cells = new RelativeBodyPoseCell[_nb_body_cells];
+
+ for(int j = 0; j < nb_body_ryc; j++) {
+ for(int i = 0; i < nb_body_rxc; i++) {
+ if(used[i + nb_body_rxc * j]) {
+ if(sq(scalar_t(i) * body_resolution + body_resolution/2 + body_rxc_min) +
+ sq(scalar_t(j) * body_resolution + body_resolution/2 + body_ryc_min) <= 1) {
+ (*global.log_stream) << "*";
+ } else {
+ (*global.log_stream) << "X";
+ }
+ } else {
+ (*global.log_stream) << ".";
+ }
+ }
+ (*global.log_stream) << endl;
+ }
+
+ int k = 0;
+ for(int j = 0; j < nb_body_ryc; j++) {
+ for(int i = 0; i < nb_body_rxc; i++) {
+
+ if(used[i + nb_body_rxc * j]) {
+
+ RelativeBodyPoseCell mother;
+
+ scalar_t x = scalar_t(i) * body_resolution + body_rxc_min;
+ scalar_t y = scalar_t(j) * body_resolution + body_ryc_min;
+
+ mother._body_xc.set(x, x + body_resolution);
+ mother._body_yc.set(y, y + body_resolution);
+
+ // scalar_t dist_min = body_resolution;
+ scalar_t dist_min = 1e6;
+
+ int nb_got;
+
+ Gaussian dist_body_radius_1, dist_body_radius_2, dist_body_tilt;
+
+ do {
+
+ nb_got = 0;
+
+ for(int t = 0; t < nb_total_targets; t++) {
+
+ scalar_t dist =
+ sqrt(sq(targets[t]._body_xc.middle() - x - body_resolution / 2) +
+ sq(targets[t]._body_yc.middle() - y - body_resolution / 2));
+
+ if(dist <= dist_min) {
+ dist_body_radius_1.add_sample(targets[t]._body_radius_1.middle());
+ dist_body_radius_2.add_sample(targets[t]._body_radius_2.middle());
+ dist_body_tilt.add_sample(targets[t]._body_tilt.middle());
+ nb_got++;
+ }
+
+ }
+
+ dist_min *= 2.0;
+ } while(nb_got < min(100, nb_total_targets));
+
+ scalar_t zeta = 4;
+
+ mother._body_radius_1.set(dist_body_radius_1.expectation() -
+ zeta * dist_body_radius_1.standard_deviation(),
+ dist_body_radius_1.expectation() +
+ zeta * dist_body_radius_1.standard_deviation());
+
+ mother._body_radius_2.set(dist_body_radius_2.expectation() -
+ zeta * dist_body_radius_2.standard_deviation(),
+ dist_body_radius_2.expectation() +
+ zeta * dist_body_radius_2.standard_deviation());
+
+ mother._body_tilt.set(dist_body_tilt.expectation() -
+ zeta * dist_body_tilt.standard_deviation(),
+ dist_body_tilt.expectation() +
+ zeta * dist_body_tilt.standard_deviation());
+
+ _body_cells[k++] = mother;
+ }
+ }
+ }
+
+ (*global.log_stream) << _nb_body_cells << " body cells." << endl;
+}
+
+PoseCellHierarchy::~PoseCellHierarchy() {
+ delete[] _body_cells;
+}
+
+int PoseCellHierarchy::nb_levels() {
+ return _nb_levels;
+}
+
+void PoseCellHierarchy::get_containing_cell(Image *image, int level,
+ Pose *pose, PoseCell *result_cell) {
+ PoseCellSet cell_set;
+
+ for(int l = 0; l < level + 1; l++) {
+ cell_set.erase_content();
+ if(l == 0) {
+ add_root_cells(image, &cell_set);
+ } else {
+ add_subcells(l, result_cell, &cell_set);
+ }
+
+ *result_cell = *(cell_set.get_containing_cell(pose));
+ }
+}
+
+void PoseCellHierarchy::add_root_cells(Image *image, PoseCellSet *cell_set) {
+
+ const int nb_scales = int((log(_max_head_radius) - log(_min_head_radius)) / log(2) *
+ global.nb_scales_per_power_of_two);
+
+ scalar_t alpha = log(_min_head_radius);
+ scalar_t beta = log(2) / scalar_t(global.nb_scales_per_power_of_two);
+
+ for(int s = 0; s < nb_scales; s++) {
+ scalar_t cell_xy_size = exp(alpha + scalar_t(s) * beta) / global.root_cell_nb_xy_per_scale;
+ PoseCell cell;
+ cell._head_radius.min = exp(alpha + scalar_t(s) * beta);
+ cell._head_radius.max = exp(alpha + scalar_t(s+1) * beta);
+ cell._head_tilt.min = -M_PI;
+ cell._head_tilt.max = M_PI;
+ for(scalar_t y = 0; y < image->height(); y += cell_xy_size)
+ for(scalar_t x = 0; x < image->width(); x += cell_xy_size) {
+ cell._head_xc.min = x;
+ cell._head_xc.max = x + cell_xy_size;
+ cell._head_yc.min = y;
+ cell._head_yc.max = y + cell_xy_size;
+ cell._body_xc.min = cell._head_xc.min - pseudo_infty;
+ cell._body_xc.max = cell._head_xc.max + pseudo_infty;
+ cell._body_yc.min = cell._head_yc.min - pseudo_infty;
+ cell._body_yc.max = cell._head_yc.max + pseudo_infty;
+ cell_set->add_cell(&cell);
+ }
+ }
+}
+
+void PoseCellHierarchy::add_subcells(int level, PoseCell *root,
+ PoseCellSet *cell_set) {
+
+ switch(level) {
+
+ case 1:
+ {
+ // Here we split the body-center coordinate cell part
+ PoseCell cell = *root;
+ scalar_t r = sqrt(cell._head_radius.min * cell._head_radius.max);
+ scalar_t x = (cell._head_xc.min + cell._head_xc.max) / 2.0;
+ scalar_t y = (cell._head_yc.min + cell._head_yc.max) / 2.0;
+ for(int k = 0; k < _nb_body_cells; k++) {
+ cell._body_xc.min = (_body_cells[k]._body_xc.min * r) + x;
+ cell._body_xc.max = (_body_cells[k]._body_xc.max * r) + x;
+ cell._body_yc.min = (_body_cells[k]._body_yc.min * r) + y;
+ cell._body_yc.max = (_body_cells[k]._body_yc.max * r) + y;
+ cell_set->add_cell(&cell);
+ }
+ }
+ break;
+
+ default:
+ {
+ cerr << "Inconsistent level in PoseCellHierarchy::add_subcells" << endl;
+ abort();
+ }
+ break;
+ }
+}
+
+
+int PoseCellHierarchy::nb_incompatible_poses(LabelledImagePool *pool) {
+ PoseCell target_cell;
+ PoseCellSet cell_set;
+ LabelledImage *image;
+
+ int nb_errors = 0;
+
+ for(int i = 0; i < pool->nb_images(); i++) {
+ image = pool->grab_image(i);
+
+ for(int t = 0; t < image->nb_targets(); t++) {
+ cell_set.erase_content();
+
+ int error_level = -1;
+
+ for(int l = 0; error_level < 0 && l < _nb_levels; l++) {
+ cell_set.erase_content();
+
+ if(l == 0) {
+ add_root_cells(image, &cell_set);
+ } else {
+ add_subcells(l, &target_cell, &cell_set);
+ }
+
+ int nb_compliant = 0;
+
+ for(int c = 0; c < cell_set.nb_cells(); c++) {
+ if(cell_set.get_cell(c)->contains(image->get_target_pose(t))) {
+ target_cell = *(cell_set.get_cell(c));
+ nb_compliant++;
+ }
+ }
+
+ if(nb_compliant != 1) {
+ error_level = l;
+ }
+ }
+
+ if(error_level >= 0) {
+ nb_errors++;
+ }
+ }
+
+ pool->release_image(i);
+ }
+
+ return nb_errors;
+}
+
+void PoseCellHierarchy::write(ostream *os) {
+ write_var(os, &_min_head_radius);
+ write_var(os, &_max_head_radius);
+ write_var(os, &_root_cell_nb_xy_per_scale);
+ write_var(os, &_nb_body_cells);
+ for(int k = 0; k < _nb_body_cells; k++)
+ write_var(os, &_body_cells[k]);
+}
+
+void PoseCellHierarchy::read(istream *is) {
+ delete[] _body_cells;
+ read_var(is, &_min_head_radius);
+ read_var(is, &_max_head_radius);
+ read_var(is, &_root_cell_nb_xy_per_scale);
+ read_var(is, &_nb_body_cells);
+ delete[] _body_cells;
+ _body_cells = new RelativeBodyPoseCell[_nb_body_cells];
+ for(int k = 0; k < _nb_body_cells; k++) {
+ read_var(is, &_body_cells[k]);
+ }
+}