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();
}
}
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;
}
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();
}
/*
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;
}
