KJB
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
mcmcda_gibbs_proposer.h
Go to the documentation of this file.
1 /* =========================================================================== *
2  |
3  | Copyright (c) 1994-2011 by Kobus Barnard (author)
4  |
5  | Personal and educational use of this code is granted, provided that this
6  | header is kept intact, and that the authorship is not misrepresented, that
7  | its use is acknowledged in publications, and relevant papers are cited.
8  |
9  | For other use contact the author (kobus AT cs DOT arizona DOT edu).
10  |
11  | Please note that the code in this file has not necessarily been adequately
12  | tested. Naturally, there is no guarantee of performance, support, or fitness
13  | for any particular task. Nonetheless, I am interested in hearing about
14  | problems that you encounter.
15  |
16  | Author: Ernesto Brau
17  * =========================================================================== */
18 
19 #ifndef MCMCDA_GIBBS_PROPOSER_H_INCLUDED
20 #define MCMCDA_GIBBS_PROPOSER_H_INCLUDED
21 
22 #include <mcmcda_cpp/mcmcda_data.h>
27 #include <sample_cpp/sample_base.h>
28 #include <m_cpp/m_vector.h>
29 #include <l_cpp/l_util.h>
30 #include <numeric>
31 #include <algorithm>
32 #include <functional>
33 #include <vector>
34 #include <set>
35 #include <utility>
36 #include <boost/function.hpp>
37 
38 namespace kjb {
39 namespace mcmcda {
40 
45 template<class Track, class Lhood>
47 {
48 private:
49  typedef typename Track::Element Element;
50  typedef std::pair<int, typename std::set<Element>::const_iterator>
51  Location_pair;
52  const Prior<Track>& m_prior;
53  const Lhood& m_likelihood;
54  Track def_track;
55  int nsize;
56  mutable double previous_prior;
57  mutable double previous_likelihood;
58 
59 public:
64  (
65  const Prior<Track>& prior,
66  const Lhood& likelihood,
67  int neighborhood_size = -1
68  ) :
69  m_prior(prior),
70  m_likelihood(likelihood),
71  nsize(neighborhood_size),
72  previous_prior(-std::numeric_limits<double>::infinity())
73  {}
74 
79  (
80  const Prior<Track>& prior,
81  const Lhood& likelihood,
82  const Track& default_track,
83  int neighborhood_size = -1
84  ) :
85  m_prior(prior),
86  m_likelihood(likelihood),
87  def_track(default_track),
88  nsize(neighborhood_size),
89  previous_prior(-std::numeric_limits<double>::infinity())
90  {}
91 
96  boost::optional<double> operator()
97  (
98  Association<Track>& w_p,
99  size_t var
100  ) const;
101 
105  bool is_track_affected
106  (
107  const Track& track,
108  const Location_pair& cur_location
109  ) const;
110 
114  Location_pair get_time_and_place
115  (
116  size_t var,
117  const Data<Element>& data
118  ) const;
119 };
120 
122 template<class Track>
124 {
125  std::vector<size_t> dims(w.get_data().size());
126  std::transform(w.get_data().begin(), w.get_data().end(), dims.begin(),
127  std::mem_fun_ref(&std::set<typename Track::Element>::size));
128 
129  return std::accumulate(dims.begin(), dims.end(), 0);
130 }
131 
132 /*============================================================================*
133  * MEMBER FUNCTION DEFINITIONS *
134  *----------------------------------------------------------------------------*/
135 
136 template<class Track, class Lhood>
137 boost::optional<double> Gibbs_proposer<Track, Lhood>::operator()
138 (
139  Association<Track>& w_p,
140  size_t var
141 ) const
142 {
143  using std::make_pair;
144  using std::swap;
145  typedef typename Track::iterator Tr_it;
146  typedef typename Track::const_iterator Tr_cit;
147 
148  const Data<Element>& tr_data = w_p.get_data();
149  Association<Track> w(tr_data);
150  swap(w, w_p);
151 
152  /*KJB(ASSERT(w.is_valid(m_prior.get_v_bar(), m_prior.get_d_bar(),
153  m_likelihood.get_convert())));*/
154 
155  Location_pair tap = get_time_and_place(var, tr_data);
156  int t = tap.first;
157  typename std::set<Element>::const_iterator point_p = tap.second;
158 
159  typename Association<Track>::const_iterator owner_p;
160  for(owner_p = w.begin(); owner_p != w.end(); owner_p++)
161  {
162  std::pair<Tr_cit, Tr_cit> pair_ps = owner_p->equal_range(t);
163  bool found = false;
164  for(Tr_cit pair_p = pair_ps.first; pair_p != pair_ps.second; pair_p++)
165  {
166  if(pair_p->second == &(*point_p))
167  {
168  found = true;
169  break;
170  }
171  }
172 
173  if(found) break;
174  }
175 
176  // Remove point from its track (making it noise)
177  // This will be used to evaluate the noise hypothesis, and will
178  // also be used as the basis for constructing the other hypotheses.
179  bool was_noise;
180  if(owner_p != w.end())
181  {
182  //w_p = Association<Track>(tr_data);
183  //w_p.clear();
184  std::remove_copy(w.begin(), w.end(),
185  std::insert_iterator<Association<Track> >(w_p, w_p.begin()),
186  *owner_p);
187 
188  Track track = *owner_p;
189  std::pair<Tr_it, Tr_it> pair_ps = track.equal_range(t);
190  for(Tr_it pair_p = pair_ps.first; pair_p != pair_ps.second; pair_p++)
191  {
192  if(pair_p->second == &(*point_p))
193  {
194  track.erase(pair_p);
195  break;
196  }
197  }
198 
199  if(!track.empty())
200  {
201  w_p.insert(track);
202  }
203 
204  was_noise = false;
205  }
206  else
207  {
208  w_p = w;
209  was_noise = true;
210  }
211 
212  std::vector<Track> affected_tracks;
213  std::remove_copy_if(w_p.begin(), w_p.end(),
214  std::back_inserter(affected_tracks),
216  this, _1, tap));
217 
218  // speed this damn thing up
219  if(nsize != -1)
220  {
221  m_likelihood.set_limits(t - nsize/2, t + nsize/2);
222  }
223 
224  // prior if assigning to noise
225  double prior_noise;
226  double likelihood_noise;
227  if(previous_prior != -std::numeric_limits<double>::infinity()
228  && was_noise && var != 0)
229  {
230  prior_noise = previous_prior;
231  likelihood_noise = previous_likelihood;
232  }
233  else
234  {
235  prior_noise = m_prior(w_p);
236  likelihood_noise = m_likelihood(w_p);
237  }
238 
239  // prior if assigning to new track
240  Track modified_track = def_track;
241  //modified_track[t] = &(*point_p);
242  modified_track.insert(make_pair(t, &(*point_p)));
244  modified_p = w_p.insert(modified_track).first;
245  double prior_new = m_prior(w_p);
246  double likelihood_new = likelihood_noise
247  - m_likelihood.at_noise_point(*point_p)
248  + m_likelihood.at_track(modified_track);
249 
250  w_p.erase(modified_p);
251 
252  /*std::cout << "single-point-noise: " << m_likelihood.at_noise_point(*point_p) << std::endl;
253  std::cout << "single-point-new: " << m_likelihood.at_track(modified_track) << std::endl;*/
254 
255  /*double prior_existing;
256  if(!affected_tracks.empty())
257  {
258  // prior if assigning to existing track
259  // (This is the same regardless of which track it gets
260  // added to, so we'll just add it to the first track.)
261  Track original_track = *affected_tracks.begin();
262  modified_track = original_track;
263  //modified_track[t] = &(*point_p);
264  modified_track.insert(make_pair(t, &(*point_p)));
265  w_p.erase(original_track);
266  modified_p = w_p.insert(modified_track).first;
267  prior_existing = m_prior(w_p);
268  w_p.erase(modified_p);
269  w_p.insert(original_track);
270  }*/
271 
272  std::vector<double> densities(affected_tracks.size() + 2);
273  std::vector<double> priors(affected_tracks.size() + 2);
274  std::vector<double> likelihoods(affected_tracks.size() + 2);
275 
276  priors[0] = prior_noise;
277  priors[1] = prior_new;
278  likelihoods[0] = likelihood_noise;
279  likelihoods[1] = likelihood_new;
280  densities[0] = prior_noise + likelihood_noise;
281  densities[1] = prior_new + likelihood_new;
282  for(size_t i = 0; i < affected_tracks.size(); i++)
283  {
284  modified_track = affected_tracks[i];
285  Track original_track = modified_track;
286  //modified_track[t] = &(*point_p);
287  modified_track.insert(make_pair(t, &(*point_p)));
288  likelihoods[i + 2] = likelihood_noise
289  - m_likelihood.at_track(affected_tracks[i])
290  - m_likelihood.at_noise_point(*point_p)
291  + m_likelihood.at_track(modified_track);
292 
293  w_p.erase(original_track);
294  modified_p = w_p.insert(modified_track).first;
295  priors[i + 2] = m_prior(w_p);
296  w_p.erase(modified_p);
297  w_p.insert(original_track);
298 
299  densities[i + 2] = priors[i + 2] + likelihoods[i + 2];
300  }
301 
302  // no if statement needed -- resetting limits is always OK
303  m_likelihood.reset_limits();
304 
305  std::vector<double> probabilities(densities.begin(), densities.end());
306  if(affected_tracks.empty())
307  {
308  probabilities[0] += log(point_p->prob_noise);
309  probabilities[1] += log(1.0 - point_p->prob_noise);
310  }
311 
312  double mx = *std::max_element(probabilities.begin(), probabilities.end());
313  std::transform(probabilities.begin(), probabilities.end(),
314  probabilities.begin(),
315  std::bind2nd(std::minus<double>(), mx));
316  std::transform(probabilities.begin(), probabilities.end(),
317  probabilities.begin(),
318  std::ptr_fun<double, double>(std::exp));
319 
320  double total = std::accumulate(probabilities.begin(),
321  probabilities.end(), 0.0);
322  std::transform(probabilities.begin(), probabilities.end(),
323  probabilities.begin(),
324  std::bind2nd(std::divides<double>(), total));
325 
326  int idx = sample(Categorical_distribution<int>(probabilities)) - 1;
327 
328  /*=======================================================*
329  * REPORTING */
330 
331  /*std::cout << "TIME: " << t << "; BOX: " << std::distance(tr_data[t - 1].begin(), point_p) << std::endl;
332  std::cout << " POSITION: " << m_likelihood.get_convert()(*point_p) << std::endl;
333  std::cout << " P-BOX-NOISE: " << point_p->prob_noise << std::endl;
334  std::cout << " NOISE: " << likelihoods[0] << " (prior: " << priors[0] << ")" " == " << probabilities[0] << std::endl;
335  std::cout << " NEW: " << likelihoods[1] << " (prior: " << priors[1] << ")" " == " << probabilities[1] << std::endl;
336  for(size_t i = 0; i < affected_tracks.size(); i++)
337  {
338  std::cout << " PART OF " << i + 1 << ": " << likelihoods[i + 2] << " (prior: " << priors[i + 2] << ")" " == " << probabilities[i + 2] << std::endl;
339  }
340  std::cout << " CHOSE: " << idx << std::endl;
341  std::cout << std::endl;*/
342 
343  /*-------------------------------------------------------*/
344 
345  if(idx == 1)
346  {
347  modified_track.clear();
348  //modified_track[t] = &(*point_p);
349  modified_track.insert(make_pair(t, &(*point_p)));
350  w_p.insert(modified_track);
351  }
352  else if(idx >= 2)
353  {
354  w_p.erase(affected_tracks[idx - 2]);
355  //affected_tracks[idx - 2][t] = &(*point_p);
356  affected_tracks[idx - 2].insert(make_pair(t, &(*point_p)));
357  w_p.insert(affected_tracks[idx - 2]);
358  }
359 
360  previous_prior = priors[idx];
361  previous_likelihood = likelihoods[idx];
362 
363  return densities[idx];
364 }
365 
366 /* \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ */
367 
368 template<class Track, class Lhood>
370 (
371  const Track& track,
372  const Location_pair& cur_location
373 ) const
374 {
375  int t = cur_location.first;
376  typename std::set<Element>::const_iterator point_p = cur_location.second;
377  const double v_bar = m_prior.get_v_bar();
378  const int d_bar = m_prior.get_d_bar();
379  int d;
380 
381  typename Track::const_iterator lb_pair_p = track.lower_bound(t);
382 
383  if(lb_pair_p == track.end())
384  {
385  typename Track::const_reverse_iterator last_pair_p = track.rbegin();
386  d = t - last_pair_p->first;
387 
388  return is_neighbor(*last_pair_p->second, *point_p, d,
389  d_bar, v_bar, m_likelihood.get_noise_sigma(),
390  m_likelihood.get_convert());
391  }
392 
393  if(lb_pair_p->first == t)
394  {
395  return is_neighbor(*point_p, *lb_pair_p->second, 0,
396  d_bar, v_bar, m_likelihood.get_noise_sigma(),
397  m_likelihood.get_convert());
398  }
399 
400  if(lb_pair_p == track.begin())
401  {
402  d = lb_pair_p->first - t;
403 
404  return is_neighbor(*point_p, *lb_pair_p->second, d,
405  d_bar, v_bar, m_likelihood.get_noise_sigma(),
406  m_likelihood.get_convert());
407  }
408 
409  typename Track::const_iterator next_pair_p = lb_pair_p--;
410  d = next_pair_p->first - t;
411  if(!is_neighbor(*next_pair_p->second, *point_p, d, d_bar, v_bar,
412  m_likelihood.get_noise_sigma(),
413  m_likelihood.get_convert()))
414  {
415  return false;
416  }
417 
418  d = t - lb_pair_p->first;
419  if(!is_neighbor(*point_p, *lb_pair_p->second, d, d_bar, v_bar,
420  m_likelihood.get_noise_sigma(), m_likelihood.get_convert()))
421  {
422  return false;
423  }
424 
425  return true;
426 }
427 
428 /* \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ */
429 
430 template<class Track, class Lhood>
431 typename Gibbs_proposer<Track, Lhood>::Location_pair
433 (
434  size_t var,
435  const Data<Element>& data
436 ) const
437 {
438  typename Data<Element>::const_iterator time_p = data.begin();
439  int t = 1;
440  size_t cur_var = 0;
441 
442  while(cur_var + time_p->size() <= var)
443  {
444  t++;
445  cur_var += time_p->size();
446  time_p++;
447  }
448 
449  typename std::set<Element>::const_iterator point_p = time_p->begin();
450  std::advance(point_p, var - cur_var);
451 
452  return std::make_pair(t, point_p);
453 }
454 
455 }} //namespace kjb::psi
456 
457 #endif /*MCMCDA_GIBBS_PROPOSER_H_INCLUDED */
458 
double accumulate(const Matrix_d< R, C, T > &mat, double init)
Definition: m_matrix_d.impl.h:432
Parent::const_iterator const_iterator
Definition: mcmcda_association.h:122
A class that holds data for the tracking problem.
Definition: mcmcda_data.h:50
void swap(Perspective_camera &cam1, Perspective_camera &cam2)
Swap two cameras.
Definition: perspective_camera.h:599
Parent::iterator iterator
Definition: mcmcda_association.h:121
Gibbs_proposer(const Prior< Track > &prior, const Lhood &likelihood, int neighborhood_size=-1)
Ctor with default-constructible track.
Definition: mcmcda_gibbs_proposer.h:64
Computes the prior of an MCMCDA association.
Definition: mcmcda_prior.h:38
A class that represents a MCMCDA association.
Definition: mcmcda_association.h:66
size_t get_association_dimension(const Association< Track > &w)
Returns the dimension of an association.
Definition: mcmcda_gibbs_proposer.h:123
size_t dims(const Scene &scene, bool respect_changed=true, bool infer_head=true)
Computes the number of variables in this scene.
Definition: pt_scene.cpp:106
Location_pair get_time_and_place(size_t var, const Data< Element > &data) const
Converts a variable index into a time and point;.
Definition: mcmcda_gibbs_proposer.h:433
const Data< Element > & get_data() const
Returns data set const-ref.
Definition: mcmcda_association.h:126
std::pair< std::vector< Track >, std::vector< size_t > > sample(const Prior< Track > &prior, size_t T, const Track &def)
Sample an association from the prior.
Definition: mcmcda_prior.h:193
void swap(kjb::Gsl_Multimin_fdf &m1, kjb::Gsl_Multimin_fdf &m2)
Swap two wrapped multimin objects.
Definition: gsl_multimin.h:693
Parent::const_iterator const_iterator
Definition: mcmcda_data.h:83
Gibbs proposal mechanism for tracking. Complies with Gibbs proposer concept.
Definition: mcmcda_gibbs_proposer.h:46
bool is_neighbor(const Element &y, const Element &y_p, int d, int d_bar, double v_bar, double sg, const typename Data< Element >::Convert &to_vector)
Returns true if given point is a neighbor of second given point.
Definition: mcmcda_data.h:182
get the indices of edges in each direction for i
Definition: APPgetLargeConnectedEdges.m:48
bool is_track_affected(const Track &track, const Location_pair &cur_location) const
Tests whether a track is affected by current variable.
Definition: mcmcda_gibbs_proposer.h:370
Definition for the Vector class, a thin wrapper on the KJB Vector struct and its related functionalit...