/* covariance matrix is linearized */
double ighmm_rand_binormal_density(const double *x, double *mean, double *cov)
{
# define CUR_PROC "ighmm_rand_binormal_density"
double rho;
#ifndef DO_WITH_GSL
double numerator,part1,part2,part3;
#endif
if (cov[0] <= 0.0 || cov[2 + 1] <= 0.0) {
GHMM_LOG(LCONVERTED, "variance <= 0.0 not allowed\n");
goto STOP;
}
rho = cov[1] / ( sqrt (cov[0]) * sqrt (cov[2 + 1]) );
/* The denominator is possibly < EPS??? Check that ? */
#ifdef DO_WITH_GSL
/* double gsl_ran_bivariate_gaussian_pdf (double x, double y, double sigma_x,
double sigma_y, double rho) */
return gsl_ran_bivariate_gaussian_pdf (x[0], x[1], sqrt (cov[0]),
sqrt (cov[2 + 1]), rho);
#else
part1 = (x[0] - mean[0]) / sqrt (cov[0]);
part2 = (x[1] - mean[1]) / sqrt (cov[2 + 1]);
part3 = m_sqr (part1) - 2 * part1 * part2 + m_sqr (part2);
numerator = exp ( -1 * (part3) / ( 2 * (1 - m_sqr(rho)) ) );
return (numerator / ( 2 * PI * sqrt(1 - m_sqr(rho)) ));
#endif
STOP:
return (-1.0);
# undef CUR_PROC
} /* double ighmm_rand_binormal_density */
void test_bivariate_gaussian(void){
double sigma_x = 3.5, sigma_y = 4.5, rho = .7, x, y, prob;
gsl_ran_bivariate_gaussian(rng, sigma_x, sigma_y, rho, &x, &y);
printf("gsl_ran_bivariate_gaussian\t[%.12f,%.12f]\t%.12f\t[%.12f,%.12f]\n",
sigma_x, sigma_y, rho, x, y);
prob = gsl_ran_bivariate_gaussian_pdf(x, y, sigma_x, sigma_y, rho);
printf("gsl_ran_bivariate_gaussian_pdf\t[%.12f,%.12f]\t[%.12f,%.12f]\t%.12f\t%.12f\n",
x, y, sigma_x, sigma_y, rho, prob);
}
double
test_bivariate_gaussian2_pdf (double y)
{
int i, n = 10 ;
double sum = 0 ;
double a = -10, b = 10, dx = (b - a)/n ;
for (i = 0; i < n ; i++)
{
double x = a + i * dx ;
sum += gsl_ran_bivariate_gaussian_pdf (x, y, 3.0, 2.0, 0.3) * dx ;
}
return sum ;
}
/**
* @brief mhtTrackingModule::calculateCosts Calculates the costs of connections for every edge possible in a specific hypothesis.
* @param hyp Hypothesis in analysis
*/
void mhtTrackingModule::calculateCosts(hypothesis& hyp)
{
double N, ux, uy, S;
QPointF x;
// The space must be reserved
int** costs = (int**) malloc((measures_t.size()+1)*sizeof(int*));
for (uint i = 0; i < measures_t.size()+1; i++)
{
costs[i] = (int*) malloc((hyp.stimation_t_1.size()+1)*sizeof(int));
}
// For every measure, and every object, calculate the cost of connection
// according to: -2log(N(z:Hx,S)) -> normal distribution -> Hx mean, using Kalman matrix,
// and S variance of the stimation and variance of the distribution.
for(uint i=0; i<measures_t.size()+1; i++)
{
if(i<measures_t.size())
{
for(uint j=0; j<hyp.stimation_t_1.size(); j++)
{
x = fixMeasure(hyp.stimation_t_1.at(j), measures_t.at(i));
S = hyp.covariance_t_1.at(j).at(j);
ux = hyp.stimation_t_1.at(j).cx;
uy = hyp.stimation_t_1.at(j).cy;
N = gsl_ran_bivariate_gaussian_pdf(x.x()-ux,x.y()-uy,S,S,0.0);
costs[i][j] = -2*log(N);
}
costs[i][hyp.stimation_t_1.size()] = CN;
}
else
{
for(uint j=0; j<hyp.stimation_t_1.size(); j++)
costs[i][j] = CD;
costs[i][hyp.stimation_t_1.size()] = 0;
}
}
delete hyp.costs;
hyp.costs = costs;
}
void GbA::get_pdf(double *pdf, int nm, int nr, double *mmarg, int nms,
double *rmarg, int nrs){
assert(nms==nm && nrs==nr);
assert(nr > 1 && nm > 1);
assert(nms == _nms && nrs == _nrs);
pthread_mutex_lock(&_process_lock);
double dm, dr, evd;
int i,j;
double *_m_tmp, *_r_tmp, *_r1, *_m1;
_m_tmp = new double[nm];
_r_tmp = new double[nr];
_r1 = new double[nr];
_m1 = new double[nm];
memcpy(_r1,_rsamples,nr*sizeof(double));
memcpy(_m1,_msamples,nm*sizeof(double));
// assume regular samples
dr = _rsamples[1] - _rsamples[0];
dm = _msamples[1] - _msamples[0];
// Compute pdf
for(i=0; i<nm; i++){
for(j=0; j<nr; j++){
pdf[i*nr+j] = gsl_ran_bivariate_gaussian_pdf(_msamples[i]-_mn[1], _rsamples[j]-_mn[0],
std::sqrt(_cov[1][1]), std::sqrt(_cov[0][0]),
_cov[0][1]/(std::sqrt(_cov[0][0])*std::sqrt(_cov[1][1])));
}
}
for(i=0;i<nm;i++){
_m_tmp[i] = 0;
for(j=0; j<nr-1; j++){
_m_tmp[i] = _m_tmp[i] + (pdf[i*nr+j] + pdf[i*nr+j+1])/2.*dr;
}
}
evd = 0;
for(i=0;i<nm-1;i++)
evd = evd + (_m_tmp[i] + _m_tmp[i+1])/2.*dm;
for(i=0;i<nm;i++)
_m_tmp[i] = _m_tmp[i] / evd;
for(j=0;j<nr;j++){
_r_tmp[j] = 0;
for(i=0; i<nm-1; i++){
_r_tmp[j] = _r_tmp[j] + (pdf[i*nr+j] + pdf[(i+1)*nr+j])/2.*dm;
}
}
evd=0;
for(i=0;i<nr-1;i++)
evd = evd + (_r_tmp[i] + _r_tmp[i+1])/2.*dr;
for(i=0;i<nr;i++)
_r_tmp[i] = _r_tmp[i] / evd;
memcpy(mmarg,_m_tmp,nm*sizeof(double));
memcpy(rmarg,_r_tmp,nr*sizeof(double));
delete[] _r1;
delete[] _m1;
delete[] _m_tmp;
delete[] _r_tmp;
pthread_mutex_unlock(&_process_lock);
}
