automatic commit
[pom.git] / pom_solver.cc
1
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.                                //
6 //                                                                              //
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.                                     //
11 //                                                                              //
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/>.         //
14 //                                                                              //
15 // Written by Francois Fleuret                                                  //
16 // (C) Ecole Polytechnique Federale de Lausanne                                 //
17 // Contact <pom@epfl.ch> for comments & bug reports                             //
18 //////////////////////////////////////////////////////////////////////////////////
19
20 #include <iostream>
21 #include <fstream>
22 #include <cmath>
23
24 using namespace std;
25
26 #include "pom_solver.h"
27 #include "global.h"
28
29 //////////////////////////////////////////////////////////////////////
30
31 POMSolver::POMSolver(Room *room) : neg(room->view_width(), room->view_height()),
32                                    neg_view(room->view_width(), room->view_height()),
33                                    ii_neg(room->view_width(), room->view_height()),
34                                    ii_neg_view(room->view_width(), room->view_height()) {
35   global_difference.set(global_mu_image_density, global_sigma_image_density);
36 }
37
38 //////////////////////////////////////////////////////////////////////
39
40 void POMSolver::compute_average_images(int camera,
41                                        Room *room,
42                                        Vector<scalar_t> *proba_absence) {
43   neg.fill(1.0);
44
45   for(int n = 0; n < room->nb_positions(); n++) if((*proba_absence)[n] <= global_proba_ignored) {
46     Rectangle *r = room->avatar(camera, n);
47     if(r->visible)
48       neg.multiply_subarray(r->xmin, r->ymin, r->xmax + 1, r->ymax + 1, (*proba_absence)[n]);
49   }
50 }
51
52 //////////////////////////////////////////////////////////////////////
53
54 void POMSolver::add_log_ratio(int camera,
55                               Room *room,
56                               ProbaView *view,
57                               Vector<scalar_t> *proba_absence,
58                               Vector<scalar_t> *sum) {
59
60   // Computes the average on the complete picture
61
62   compute_average_images(camera, room, proba_absence);
63
64   double s = ii_neg.compute_sum(&neg);
65   double sv = ii_neg_view.compute_sum(&neg, view);
66
67   scalar_t noise_proba = 0.01; // 1% of the scene can remain unexplained
68   scalar_t average_surface = room->view_width() * room->view_height() * (1 + noise_proba) - s;
69   scalar_t average_diff = average_surface + sv;
70
71   // Cycles throw all positions and adds the log likelihood ratio to
72   // the total sum for each
73
74   for(int i = 0; i < room->nb_positions(); i++) {
75     Rectangle *r = room->avatar(camera, i);
76     if(r->visible) {
77       scalar_t lambda = 1 - 1/(*proba_absence)[i];
78
79       scalar_t integral_neg = ii_neg.integral(r->xmin, r->ymin, r->xmax + 1, r->ymax + 1);
80       scalar_t average_surface_givpre = average_surface +          integral_neg;
81       scalar_t average_surface_givabs = average_surface + lambda * integral_neg;
82
83       scalar_t integral_neg_view = ii_neg_view.integral(r->xmin, r->ymin, r->xmax + 1, r->ymax + 1);
84       scalar_t average_diff_givpre = average_diff +           integral_neg - 2 * integral_neg_view;
85       scalar_t average_diff_givabs = average_diff + lambda * (integral_neg - 2 * integral_neg_view);
86
87       scalar_t log_mu0 = global_difference.log_proba(average_diff_givabs / average_surface_givabs);
88       scalar_t log_mu1 = global_difference.log_proba(average_diff_givpre / average_surface_givpre);
89
90       (*sum)[i] += log_mu1 - log_mu0;
91
92     }
93   }
94 }
95
96 void POMSolver::solve(Room *room,
97                       Vector<scalar_t> *prior,
98                       Vector<ProbaView *> *views,
99                       Vector<scalar_t> *result_proba_presence,
100                       int nb_frame,
101                       char *convergence_file_format) {
102
103   Vector<scalar_t> log_prior_ratio(prior->length());
104
105   Vector<scalar_t> sum(room->nb_positions());
106   Vector<scalar_t> proba_absence(room->nb_positions());
107
108   for(int i  = 0; i < room->nb_positions(); i++) {
109     log_prior_ratio[i] = log((*prior)[i]/(1 - (*prior)[i]));
110     proba_absence[i] = 1 - (*prior)[i];
111   }
112
113   int nb_stab = 0;
114
115   for(int it = 0; (nb_stab < global_nb_stable_error_for_convergence) && (it < global_max_nb_solver_iterations); it++) {
116
117     sum.clear();
118     for(int c = 0; c < room->nb_cameras(); c++)
119       add_log_ratio(c, room, (*views)[c], &proba_absence, &sum);
120
121     scalar_t e = 0;
122     for(int i = 0; i < room->nb_positions(); i++) {
123       scalar_t np = global_smoothing_coefficient * proba_absence[i] +
124         (1 - global_smoothing_coefficient) / (1 + exp(log_prior_ratio[i] + sum[i]));
125       if(abs(proba_absence[i] - np) > e) e = abs(proba_absence[i] - np);
126       proba_absence[i] = np;
127     }
128
129     if(e < global_error_max) nb_stab++; else nb_stab = 0;
130
131     if(convergence_file_format) {
132       char buffer[buffer_size];
133       for(int p = 0; p < room->nb_positions(); p++) {
134         (*result_proba_presence)[p] = 1 - proba_absence[p];
135       }
136
137       for(int c = 0; c < room->nb_cameras(); c++) {
138         pomsprintf(buffer, buffer_size, convergence_file_format, c, nb_frame, it);
139         cout << "Saving " << buffer << "\n"; cout.flush();
140         room->save_stochastic_view(buffer, c, (*views)[c], result_proba_presence);
141       }
142     }
143
144   }
145
146   for(int p = 0; p < room->nb_positions(); p++) {
147     (*result_proba_presence)[p] = 1 - proba_absence[p];
148   }
149 }