pt_sample_scenes.h

Go to the documentation of this file.

/* =========================================================================== *
|
|  Copyright (c) 1994-2008 by Kobus Barnard (author).
|
|  Personal and educational use of this code is granted, provided that this
|  header is kept intact, and that the authorship is not misrepresented, that
|  its use is acknowledged in publications, and relevant papers are cited.
|
|  For other use contact the author (kobus AT cs DOT arizona DOT edu).
|
|  Please note that the code in this file has not necessarily been adequately
|  tested. Naturally, there is no guarantee of performance, support, or fitness
|  for any particular task. Nonetheless, I am interested in hearing about
|  problems that you encounter.
|
| Authors:
|     Ernesto Brau, Jinyan Guan
|
* =========================================================================== */

/* $Id$ */

#ifndef PT_SAMPLE_SCENES_H
#define PT_SAMPLE_SCENES_H

#include <people_tracking_cpp/pt_scene.h>
#include <people_tracking_cpp/pt_target.h>
#include <people_tracking_cpp/pt_association.h>
#include <people_tracking_cpp/pt_scene_adapter.h>
#include <people_tracking_cpp/pt_scene_posterior.h>
#include <people_tracking_cpp/pt_recorder.h>
#include <prob_cpp/prob_distribution.h>
#include <l_cpp/l_exception.h>
#include <vector>
#include <algorithm>
#include <boost/foreach.hpp>
#include <boost/optional.hpp>

#ifdef KJB_HAVE_ERGO
#include <ergo/hmc.h>
#include <ergo/mh.h>
#include <ergo/record.h>
#endif

namespace kjb {
namespace pt {

//struct Control_scene
//{
//    Scene scene;
//};

bool all_new_targets(const Scene& scene);

class Propose_person_size
{
public:
    Propose_person_size
    (
        double hsdv, 
        double wsdv, 
        double gsdv, 
        bool infer_head = true
    ) 
        : N_height(0.0, hsdv), 
          N_width(0.0, wsdv), 
          N_girth(0.0, gsdv),
          m_infer_head(infer_head)
    {}

#ifdef KJB_HAVE_ERGO
    ergo::mh_proposal_result operator()(const Scene& in, Scene& out);
#endif

private:
    Normal_distribution N_height;
    Normal_distribution N_width;
    Normal_distribution N_girth;
    bool m_infer_head;
};

class Sample_scenes
{
public:
    enum Sample_method { HMC, MH };
    typedef ergo::hmc_step<Scene> Hmc_step;
    typedef ergo::mh_step<Scene> Mh_step;

public:
    Sample_scenes(const Scene_posterior& posterior, bool infer_head) :
        posterior_p_(&posterior),
        sample_method_(HMC),
        num_iterations_(10),
        num_leapfrog_steps_(5),
        pos_opt_step_size_(0.00001),
        pos_grad_step_size_(0.01),
        estimate_grad_step_size_(true),
        height_sdv_(0.02),
        width_sdv_(0.01),
        girth_sdv_(0.01),
        infer_head_(infer_head)
    {}

public:
    const Scene_posterior& scene_posterior() const { return *posterior_p_; }

    void set_scene_posterior(const Scene_posterior& post)
    {
        posterior_p_ = &post;
    }

    void set_sample_method(Sample_method method) { sample_method_ = method; }

    void set_num_iterations(size_t num_its) { num_iterations_ = num_its; }

    void set_num_leapfrog_steps(size_t lf_steps)
    {
        num_leapfrog_steps_ = lf_steps;
    }

    void set_hmc_step_size(double pss) { pos_opt_step_size_ = pss; }

    void set_gradient_step_size(double pss)
    {
        pos_grad_step_size_ = pss;
        estimate_grad_step_size_ = false;
    }

    void set_size_proposal_sigmas(double hsdv, double wsdv, double gsdv)
    {
        height_sdv_ = hsdv;
        width_sdv_ = wsdv;
        girth_sdv_ = gsdv;
    }

    template<
        class StepIterator,
        class SampleIterator,
        class ProposalIterator,
        class PdIterator>
    void operator()
    (
        const Scene& initial_scene,
        boost::optional<StepIterator> step_out,
        boost::optional<SampleIterator> sample_out,
        boost::optional<ProposalIterator> proposed_out,
        boost::optional<PdIterator> pd_out
    ) const;

private:
#ifdef KJB_HAVE_ERGO

    Hmc_step make_hmc_traj_step
    (
        const Scene_adapter& scene_adapter,
        Scene& best_scene
    ) const;

//    /** @brief  Helper function that creates an MH trajectory step. */
//    Mh_step make_mh_traj_step() const;

    Mh_step make_mh_size_step(bool infer_head) const;

    template<class TrajStep, class TRecIter, class SRecIter>
    void run_steps
    (
        Scene& scene,
        double& lt,
        //const TrajStep& traj_step,
        TrajStep& traj_step,
        TRecIter tfirst,
        TRecIter tlast,
        const Mh_step& size_step,
        SRecIter sfirst,
        SRecIter slast
    ) const;
#endif

private:
    const Scene_posterior* posterior_p_;
    Sample_method sample_method_;
    size_t num_iterations_;
    size_t num_leapfrog_steps_;
    double pos_opt_step_size_;
    double pos_grad_step_size_;
    bool estimate_grad_step_size_;
    double height_sdv_;
    double width_sdv_;
    double girth_sdv_;
    bool infer_head_;
};

/* \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ */

template<
    class StepIterator,
    class SampleIterator,
    class ProposalIterator,
    class PdIterator>
void Sample_scenes::operator()
(
    const Scene& initial_scene,
    boost::optional<StepIterator> step_out,
    boost::optional<SampleIterator> sample_out,
    boost::optional<ProposalIterator> proposed_out,
    boost::optional<PdIterator> pd_out
) const
{
#ifdef KJB_HAVE_ERGO
    typedef std::vector<Hmc_step::record_t> Hmc_rec_vector;
    typedef std::vector<Mh_step::record_t> Mh_rec_vector;

    if(num_iterations_ == 0) return;

    // size of scene
    Scene_adapter scene_adapter(posterior_p_->vis_off(), infer_head_);
    size_t scene_size = scene_adapter.size(&initial_scene);

    if(scene_size == 0) return;

    // make current and best scene and posterior value
    Scene cur_scene = initial_scene;
    double cur_lt = (*posterior_p_)(cur_scene);
    Scene best_scene = initial_scene;

    // steps
    Hmc_step traj_step = make_hmc_traj_step(scene_adapter, best_scene);
    Mh_step size_step = make_mh_size_step(infer_head_);

    // recorders
    Hmc_rec_vector hrv;
    Mh_rec_vector mrv;
    hrv.reserve(4);
    mrv.reserve(4);

    // best scene
    hrv.push_back(ergo::make_best_sample_recorder(&best_scene).replace());
    mrv.push_back(ergo::make_best_sample_recorder(&best_scene).replace());

    // log recorder
    if(step_out)
    {
        hrv.push_back(ergo::make_hmc_detail_recorder(*step_out));
        mrv.push_back(ergo::make_mh_detail_recorder(*step_out));
    }

    // sample recorder
    if(sample_out)
    {
        hrv.push_back(ergo::make_sample_recorder(*sample_out));
        mrv.push_back(ergo::make_sample_recorder(*sample_out));
    }

    // proposal recorder
    if(proposed_out)
    {
        hrv.push_back(ergo::make_proposed_recorder(*proposed_out));
        mrv.push_back(ergo::make_proposed_recorder(*proposed_out));
        traj_step.store_proposed();
        size_step.store_proposed();
    }

    // proposal detail recorder
    if(pd_out)
    {
        hrv.push_back(make_pd_recorder(*pd_out, *posterior_p_));
        mrv.push_back(make_pd_recorder(*pd_out, *posterior_p_));
        traj_step.store_proposed();
        size_step.store_proposed();
    }

    // run steps
    run_steps(
        cur_scene,
        cur_lt,
        traj_step,
        hrv.begin(),
        hrv.end(),
        size_step,
        mrv.begin(),
        mrv.end());

    // swap in best trajectories
    Ascn::const_iterator tg_p = best_scene.association.begin();
    BOOST_FOREACH(const Target& itarget, initial_scene.association)
    {
        const Target& btarget = *tg_p++;

        using std::swap;
        swap(btarget.trajectory(), itarget.trajectory());
        swap(btarget.body_trajectory(), itarget.body_trajectory());
        swap(btarget.face_trajectory(), itarget.face_trajectory());
    }

#else
    KJB_THROW_2(Missing_dependency, "libergo");
#endif
}

/* \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ \/ */

#ifdef KJB_HAVE_ERGO

template<class TrajStep, class TRecIter, class SRecIter>
void Sample_scenes::run_steps
(
    Scene& scene,
    double& lt,
    //const TrajStep& traj_step,
    TrajStep& traj_step,
    TRecIter tfirst,
    TRecIter tlast,
    const Mh_step& size_step,
    SRecIter sfirst,
    SRecIter slast
) const
{
    bool rss = all_new_targets(scene);

    int D = dims(scene, true);
    //size_t num_iterations = std::max(5, D / 20); 
    //size_t num_iterations = std::max(5, D / 5); 
    size_t num_iterations = std::max(5, D / 10); 
    std::cout << "\t\t\t\tscene size: " << std::setw(4) << D << " iters: " << num_iterations << std::endl;
    double best_lt = lt; 
    double prev_best_lt = lt; 
    for(size_t i = 1; i <= num_iterations; i++)
    {
        // run hmc step and record
        traj_step(scene, lt);
        /*if(traj_step.acceptance_probability() < -10)
        {
            std::vector<double> step_sizes = traj_step.step_sizes();
            if(step_sizes[0] > 1e-5)
            {
                double ratio = std::pow((i+1), 2.0);
                std::transform(step_sizes.begin(),
                               step_sizes.end(),
                               step_sizes.begin(),
                               std::bind2nd(std::divides<double>(), ratio));
                traj_step.reset(step_sizes);
                std::cout << "new step: " << step_sizes[0] << std::endl;
            }
        }*/
        for(TRecIter rec_p = tfirst; rec_p != tlast; ++rec_p)
        {
            (*rec_p)(traj_step, scene, lt);
        }

        // run mh step and record
        if(rss)
        {
            size_step(scene, lt);
            for(SRecIter rec_p = sfirst; rec_p != slast; ++rec_p)
            {
                (*rec_p)(size_step, scene, lt);
            }
        }
        if(lt > best_lt) best_lt = lt;

        if((i + 1) % 10 == 0) 
        {
            if(best_lt > prev_best_lt)
                prev_best_lt = best_lt;
            else break;
        }
    }
}

#endif

}} // namespace kjb::pt

#endif /*PT_SAMPLE_SCENES_H */