Esempio n. 1
0
void nested_mpi::take_step(){

  for (int i=0;i<numProcs;++i){
    int ii=pre_log_posterior.min_x();
    if (cpu_log_posterior.x_data(i) > min_log_like){
      for(int j=0;j<nb_var;++j) {param_vector[j]=pre_chain[j].x_data(ii);}
      for(int j=0;j<nb_var;++j) {chains[j].set(param_vector[j]);}
      log_posterior.set(pre_log_posterior.x_data(ii));
      weights.set(1.0);
      for(int j=0;j<nb_var;++j) 
        pre_chain[j].set(ii,cpu_chain[j].x_data(i));
      pre_log_posterior.set(ii,cpu_log_posterior.x_data(i));
      min_log_like=pre_log_posterior.min();
      size_chain++;dump_nb_mpi++;conv_nb_mpi++;cov_nb_mpi=0;steps_taken++;
    } else {cov_nb_mpi++;}
  }

  for(int j=0;j<nb_var;++j) cpu_chain[j].clear();
  cpu_log_posterior.clear();
  if (cov_nb_mpi >= cov_lenght) {
    cov_nb_mpi = 0;
    calc_covariance_pre();
    update_covariance();
  }
}
Esempio n. 2
0
uint8_t kalman_update(
        kalman_robot_handle_t * handle,
        const position_t * measurement,
        float delta_t,
        robot_pos_t * dest)
{
    if(handle == NULL || dest == NULL || delta_t < 0.0f) {
        return 0;
    }

    os_mutex_take(&(handle->_mutex));

    // predict new state
    predict_state(&(handle->_state), delta_t, &(handle->_state));

    // predict new covariance
    covariance_t proc_noise_cov;
    process_noise_covariance(handle, delta_t, &proc_noise_cov);
    predict_covariance(
            &(handle->_state_covariance),
            &proc_noise_cov,
            delta_t,
            &(handle->_state_covariance));

    // if there is a measurement make kalman update
    if(measurement != NULL) {
        // compute kalman gain
        kalman_gain_t gain;
        kalman_gain(
                &(handle->_state_covariance),
                &(handle->_measurement_covariance),
                &gain);

        // compute difference between prediction and measurement
        vec2d_t residual;
        residual._x = measurement->x - handle->_state._x;
        residual._y = measurement->y - handle->_state._y;

        // estimate new state considering measurement
        update_state(&(handle->_state), &gain, &residual, &(handle->_state));

        update_covariance(
                &(handle->_state_covariance),
                &gain,
                &(handle->_state_covariance));
    }

    // write resulting position (and associated variances) to dest
    dest->x = handle->_state._x;
    dest->y = handle->_state._y;
    dest->var_x = handle->_state_covariance._cov_a._a;
    dest->var_y = handle->_state_covariance._cov_a._d;
    dest->cov_xy = handle->_state_covariance._cov_a._b;

    os_mutex_release(&(handle->_mutex));

    return 1;
}
Esempio n. 3
0
void nested_mpi::make_pre_chain(){

  choose_cpu_pre_step(); 
  param_log_like=pre_log_posterior.min();

  calc_covariance_pre(); 
  update_covariance();

  if (myid == 0){
    std::string name_here;
    name_here=tag_name+"_pre_chain";
    save_pre(name_here);
    name_here=tag_name+"_pre_cov";
    save_cov(name_here);
  }

  int ii=pre_log_posterior.max_x();
  for (int i=0;i<nb_var;++i) param_max[i]=pre_chain[i].x_data(ii);
  max_log_like=pre_log_posterior.x_data(ii);
  min_log_like=pre_log_posterior.min();

}
Esempio n. 4
0
/*

  INPUTS      :
  imgx         Spatial gradients under x.
  imgy         Spatial gradients under y.
  imgt         Temporal gradient (DFD): imgt = I(pi+depl,t+1) - I(pi,t)
  zone_val     Estimation support.
  etiq	       Support value to take into consderation.
  fen          Work window.
  ima_pond     Ponderation.

  OUTPUT      :
  d_param      Estimated motion model in the polynomial form.
 
  DESCRIPTION :
  Compute a robust estimation of the motion model.

  RETURN      :
  The number of pixels used for the computation.
  
*/
bool estimate_QUA_COMPLET(TImageFloat *imgx, TImageFloat *imgy,
			 TImageFloat *imgt, TImageShort *zone_val,
			 int etiq, TWindow win,
			 TImageFloat *ima_pond, Para *d_param)
{
  int           N; // Number of parameters

  N = d_param->nb_para;
  if (d_param->var_light)
    N++;

  int		cnt = 0;		// Pixel counter
  int		nb_pts_utiles = 0;	// Used pixel counter

  double	YTWY	  = 0.0;
  double	YTWXTheta = 0.0;

  double        *A_  = new double [N*N];
  double        **A  = new double* [N];
  double        *B   = new double [N];
  double        *X   = new double [N];
  double        *phi = new double [N];
  double        li_c = d_param->li_c;
  double        co_c = d_param->co_c;

  int		success = 0;	// return value
  int		x;		// column
  int		y;		// row
  int		i, j;
  
  for (i=0; i < N; i ++)
    A[i] = A_ + i*N;

  d_param->sigma2res = 0.0;
  set_double (X, 0.0, N);
  set_double (B, 0.0, N);
  set_double (&A[0][0], 0.0, N * N);

  // Diagonal pertubation, to assume that the matrix would be inversible
  for (i = 0; i < N; i++)
    A[i][i] = PERTURBATION;

  if (d_param->var_light)
    phi[N-1] = 1.0;  // For the global illumination variation parameter 

  for (y = win.dli; y <win.fli; y++)  {
    size_t	off   = MIJ(0, y, 0, ima_pond->nbco);
    short	*pval = &zone_val->ad[off];
    float	*pond = &ima_pond->ad[off];
    float	*pgx  = &imgx->ad[off];
    float	*pgy  = &imgy->ad[off];
    float	*pgt  = &imgt->ad[off];

    double	dy = (double) (y - li_c);

    for(x = win.dco; x < win.fco; x++) {
      double	dpond, dpgt;	// temporary values
      double	dx;

      if ((pval[x]==etiq) && (((pgx[x]!=0.0)||(pgy[x]!=0.0))||(pgt[x]!=0.0))){
	nb_pts_utiles++;
	if (pond[x] > POND_MINI) {
	  cnt++;

	  dpond = (double) pond[x];
	  dpgt  = (double) pgt[x];
	  dx    = (double) (x - co_c);

  	  phi[0]  = pgx[x];
	  phi[1]  = pgy[x];
	  phi[2]  = phi[0] * dx;
	  phi[3]  = phi[0] * dy;
	  phi[4]  = phi[1] * dx;
	  phi[5]  = phi[1] * dy;
	  phi[6]  = phi[2] * dx;
	  phi[7]  = phi[2] * dy;
	  phi[8]  = phi[3] * dy;
	  phi[9]  = phi[4] * dx;
	  phi[10] = phi[4] * dy;
	  phi[11] = phi[5] * dy;

	  // For the residual variance computation
	  if (d_param->compute_sigma2res)
	    YTWY += dpond * dpgt * dpgt;

	  for (i = 0; i < N; i++)  {
	    double	d = (double) phi[i] * dpond;

	    B[i] -= d * dpgt;
	    // Cumulation on a triangle matrix
	    for (j = 0; j <= i; j++)
	      A[i][j] +=  phi[j] * d;
	  }
	}
      }
    }
  }

  // Fill the upper triangle of the matrix
  for (i = 0; i < N; i++)
    for (j = i + 1; j < N; j++)
      A[i][j] = A[j][i];

  d_param->tx_pts = ((double)cnt)/nb_pts_utiles;

  if (cnt <= N * 2) {	// Not enough pixels
    delete [] A_;
    delete [] A;
    delete [] B;
    delete [] X;
    delete [] phi;
    return false;
  }

  // Solve AX = B by exploiting the symetry of the matrix
  success = resoud_mat_sym (&A[0][0], B, X, N, N);

  if (! success) {
    set_double (X, 0.0, N);
    return false;
  }

  // Convert the compact form to the polynomial form
  if (d_param->var_light) {
    for (i=0; i < N-1; i ++)
      d_param->thet[i] = X[i];
    d_param->thet[12] = X[N-1]; // Illumination variation
  }
  else {
    for (i=0; i < N; i ++)
      d_param->thet[i] = X[i];
  }
 
  if ( ! d_param->compute_sigma2res) {
    delete [] A_;
    delete [] A;
    delete [] B;
    delete [] X;
    delete [] phi;
    return true;
  }

  // Compute the residual variance
  YTWXTheta = dot_double (B, X, N);
  d_param->sigma2res = (YTWY - YTWXTheta) / (double) cnt;

  if ( ! d_param->compute_covariance) {
    delete [] A_;
    delete [] A;
    delete [] B;
    delete [] X;
    delete [] phi;
    return true;
  }

  // Compute the covariance matrix
  double *InvA_  = new double [N*N];
  double **InvA  = new double* [N];
  for (i=0; i < N; i ++)
    InvA[i] = InvA_ + i*N;

  success = inverse_mat_sym (&A[0][0], &InvA[0][0], N);
  if (! success) {
    return false;
  }
  update_covariance(&InvA[0][0], d_param);

  delete [] A_;
  delete [] A;
  delete [] B;
  delete [] X;
  delete [] phi;
  delete [] InvA_;
  delete [] InvA;

  return true;
}