pt_position_prior.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.
|
* =========================================================================== */

/* $Id: pt_position_prior.h 18921 2015-04-20 23:26:14Z ernesto $ */

#ifndef PT_POSITION_PRIOR_H_
#define PT_POSITION_PRIOR_H_

#include <people_tracking_cpp/pt_scene.h>
#include <people_tracking_cpp/pt_target.h>
#include <gp_cpp/gp_prior.h>
#include <gp_cpp/gp_normal.h>

namespace kjb {
namespace pt {

class Position_prior
{
private:
    typedef gp::Prior<gp::Zero, gp::Squared_exponential> Gpp;

public:
    Position_prior(double sc, double sv, size_t local_sz) :
        gpsc_(sc),
        gpsv_(sv),
        local_dist_(
            gp::Zero(),
            gp::Sqex(gpsc_, gpsv_),
            gp::Inputs::const_iterator(),
            gp::Inputs::const_iterator()),
        sweight_(25.0),
        m_local_sz(local_sz)
    {
        if(m_local_sz % 2 == 0)
        {
            m_local_sz++;
        }

        if(m_local_sz <= 1)
        {
            m_local_sz = 3;
        }
    }

    Position_prior(double sc, double sv) :
        gpsc_(sc),
        gpsv_(sv),
        local_dist_(
            gp::Zero(),
            gp::Sqex(gpsc_, gpsv_),
            gp::Inputs::const_iterator(),
            gp::Inputs::const_iterator()),
        sweight_(25.0),
        m_local_sz(8*gpsc_)
    {
        if(m_local_sz % 2 == 0)
        {
            m_local_sz++;
        }

        if(m_local_sz <= 1)
        {
            m_local_sz = 3;
        }
    }

    double operator()(const Scene& scene) const;

    double at_space(const Scene& scene) const;

    double at_trajectory(const Target& target) const;

    double local(const Target& target, size_t t) const;

    double local_space(const Scene& scene, const Target& target, size_t t) const;

    size_t local_size() const { return m_local_sz; }

    double scale() const { return gpsc_; }

    double signal_variance() const { return gpsv_; }

private:
    double gpsc_;
    double gpsv_;
    mutable Gpp local_dist_;
    double sweight_;
    size_t m_local_sz;
};

}} //namespace kjb::pt

#endif /*PT_POSITION_PRIOR_H_ */