compute_prob1.cpp

Go to the documentation of this file.

/*
 * compute_prob1.cpp
 *
 * Code generation for function 'compute_prob1'
 *
 * C source code generated on: Wed Jul 24 16:11:35 2013
 *
 */

/* Include files */
#include "rt_nonfinite.h"
#include "Optimal_affine_tracking_3d16_fast_realtime.h"
#include "compute_prob1.h"
#include "norm.h"
#include "estimateRigidTransform.h"
#include "mean.h"
#include "image_warping.h"
#include "mldivide.h"
#include "expm.h"
#include "randn.h"
#include "chol.h"
#include "sum.h"
#include "diag.h"
#include "log.h"
#include "eig.h"
#include "mrdivide.h"
#include "Optimal_affine_tracking_3d16_fast_realtime_emxutil.h"
#include "removeOutliers.h"
#include "round.h"
#include "resampling.h"
#include "exp.h"
#include "repmat.h"
#include "gradient.h"
#include "Optimal_affine_tracking_3d16_fast_realtime_rtwutil.h"

/* Type Definitions */

/* Named Constants */

/* Variable Declarations */

/* Variable Definitions */

/* Function Declarations */
static void b_eml_li_find(const boolean_T x[25], int32_T y_data[25], int32_T
  y_size[1]);
static int32_T compute_nones(const boolean_T x[1296]);
static real_T rt_remd_snf(real_T u0, real_T u1);

/* Function Definitions */
static void b_eml_li_find(const boolean_T x[25], int32_T y_data[25], int32_T
  y_size[1])
{
  int32_T k;
  int32_T i;
  k = 0;
  for (i = 0; i < 25; i++) {
    if (x[i]) {
      k++;
    }
  }

  y_size[0] = k;
  k = 0;
  for (i = 0; i < 25; i++) {
    if (x[i]) {
      y_data[k] = i + 1;
      k++;
    }
  }
}

static int32_T compute_nones(const boolean_T x[1296])
{
  int32_T k;
  int32_T i;
  k = 0;
  for (i = 0; i < 1296; i++) {
    if (x[i]) {
      k++;
    }
  }

  return k;
}

static real_T rt_remd_snf(real_T u0, real_T u1)
{
  real_T y;
  boolean_T b_y;
  boolean_T c_y;
  real_T tr;
  if (u1 < 0.0) {
    y = ceil(u1);
  } else {
    y = floor(u1);
  }

  b_y = ((!rtIsNaN(u0)) && (!rtIsInf(u0)));
  c_y = ((!rtIsNaN(u1)) && (!rtIsInf(u1)));
  if ((u1 != 0.0) && (u1 != y) && (b_y && c_y)) {
    tr = u0 / u1;
    if (fabs(tr - rt_roundd_snf(tr)) <= DBL_EPSILON * fabs(tr)) {
      y = 0.0;
    } else {
      y = fmod(u0, u1);
    }
  } else {
    y = fmod(u0, u1);
  }

  return y;
}

void compute_prob1(real_T X_par[2250000], real_T X_par_pred[2250000], real_T
                   AR_velocity[2250000], const real_T dw_dp[15552], real_T t,
                   real_T center_x, real_T center_y, const real_T Ixyz[504063],
                   const real_T point_matrix[3888], real_T mean_img[3888],
                   real_T tracked_images[38880000], real_T Aff_matrix[9], real_T
                   centroid[3])
{
  static real_T warped_img[3888];
  static real_T frame[504063];
  static real_T b_Ixyz[168021];
  static real_T c_Ixyz[168021];
  int32_T ix;
  int32_T i26;
  static real_T ext_frame[1008126];
  static real_T ext_grad_x[336042];
  static real_T ext_grad_y[336042];
  real_T rigid_transform_par[400];
  real_T dist1Array[25];
  int32_T par;
  creal_T LOGMX[9];
  creal_T X_par_temp[9];
  creal_T dcv0[3];
  creal_T dcv1[9];
  creal_T b_X_par_temp[9];
  creal_T c_X_par_temp[9];
  real_T d;
  real_T s;
  int32_T i;
  real_T b_center_x[2];
  static const int8_T iv10[3] = { 0, 0, 1 };

  boolean_T zero_ind_z[1296];
  boolean_T b_zero_ind_z;
  boolean_T c_zero_ind_z[1296];
  int32_T tmp_size[1];
  int32_T tmp_data[1296];
  int32_T b_tmp_size[1];
  int32_T b_tmp_data[1296];
  real_T warped_grad_x3[1296];
  int32_T warped_img_size[1];
  int32_T b_warped_img_size[1];
  int32_T c_warped_img_size[1];
  real_T warped_grad_y3[1296];
  real_T img_grad[2592];
  real_T transform[16];
  static real_T b_warped_img[5184];
  static real_T b_transform[5184];
  static creal_T a[7776];
  static real_T b_a[7776];
  real_T c_a[2592];
  creal_T sum_jacobian[6];
  creal_T increment[6];
  creal_T d_X_par_temp[6];
  creal_T e_X_par_temp[6];
  creal_T dcv2[6];
  creal_T dcv3[6];
  creal_T f_X_par_temp[36];
  static creal_T d_a[7776];
  static const real_T dv13[36] = { 0.00066666666666666675, 0.0, 0.0, 0.0, 0.0,
    0.0, 0.0, 2.6666666666666667E-5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
    0.002666666666666667, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.6666666666666667E-5,
    0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.2666666666666666, 0.0, 0.0, 0.0, 0.0, 0.0,
    0.0, 3.2666666666666666 };

  creal_T dcv4[36];
  creal_T sigma_12[6];
  creal_T y[6];
  real_T sigma_22_re;
  real_T sigma_22_im;
  real_T b;
  real_T sigma_12_re;
  real_T brm;
  static const int8_T b_b[9] = { 0, 0, 0, 0, 0, 0, 0, 1, 0 };

  static const int8_T c_b[9] = { 0, 0, 0, 0, 0, 0, 1, 0, 0 };

  static const int8_T d_b[9] = { 0, 1, 0, 1, 0, 0, 0, 0, 0 };

  static const int8_T e_b[9] = { 0, 1, 0, -1, 0, 0, 0, 0, 0 };

  static const int8_T f_b[9] = { 1, 0, 0, 0, -1, 0, 0, 0, 0 };

  static const int8_T g_b[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 0 };

  real_T CH[36];
  real_T h_b[6];
  real_T random_coeff[6];
  real_T b_random_coeff[9];
  real_T i_b[9];
  int32_T d_warped_img_size[1];
  int32_T e_warped_img_size[1];
  int32_T f_warped_img_size[1];
  static real_T c_transform[5184];
  static real_T warped_img_tr[3888];
  static real_T b_warped_img_tr[3888];
  boolean_T b_dist1Array[25];
  int32_T c_tmp_data[25];
  real_T outindex[25];
  real_T b_X_par_pred[225];
  real_T b_rigid_transform_par[400];
  int32_T itmp;
  boolean_T exitg1;
  real_T mean_log[4];
  real_T c_X_par_pred[4];
  real_T b_X_par[4];
  real_T b_LOGMX[4];
  creal_T D[4];
  creal_T V[4];
  creal_T dcv5[2];
  creal_T b_V[4];
  real_T c_X_par[50];
  real_T mean_trans[2];
  int32_T g_warped_img_size[1];
  int32_T h_warped_img_size[1];
  int32_T i_warped_img_size[1];
  static real_T c_rigid_transform_par[5184];
  emxArray_real_T *b_tracked_images;
  emxArray_real_T *c_tracked_images;
  emxArray_real_T *d_tracked_images;
  memset(&warped_img[0], 0, 3888U * sizeof(real_T));
  memset(&frame[0], 0, 504063U * sizeof(real_T));
//#pragma omp for schedule (dynamic, 8)
  for (ix = 0; ix < 441; ix++) {
    for (i26 = 0; i26 < 381; i26++) {
      frame[336042 + (i26 + 381 * ix)] = Ixyz[336042 + (i26 + 381 * ix)];
      b_Ixyz[i26 + 381 * ix] = (Ixyz[i26 + 381 * ix] + 2.0) * (real_T)(frame
        [336042 + (i26 + 381 * ix)] != 0.0);
      frame[i26 + 381 * ix] = b_Ixyz[i26 + 381 * ix];
      c_Ixyz[i26 + 381 * ix] = (Ixyz[168021 + (i26 + 381 * ix)] + 2.0) * (real_T)
        (frame[336042 + (i26 + 381 * ix)] != 0.0);
      frame[168021 + (i26 + 381 * ix)] = c_Ixyz[i26 + 381 * ix];
    }
  }

  repmat(frame, ext_frame);
  gradient(*(real_T (*)[168021])&frame[336042], b_Ixyz, c_Ixyz);
  b_repmat(c_Ixyz, ext_grad_x);
  b_repmat(b_Ixyz, ext_grad_y);
//#pragma omp for schedule (dynamic, 80)
  for (par = 0; par < 25; par++) {
    eig(*(real_T (*)[9])&AR_velocity[9 * par + 225 * ((int32_T)(t - 1.0) - 1)],
        X_par_temp, LOGMX);
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 3; ix++) {
      dcv0[ix] = LOGMX[ix << 2];
    }

    b_log(dcv0);
    diag(dcv0, dcv1);
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 3; ix++) {
      for (i26 = 0; i26 < 3; i26++) {
        b_X_par_temp[i26 + 3 * ix].re = X_par_temp[ix + 3 * i26].re;
        b_X_par_temp[i26 + 3 * ix].im = -X_par_temp[ix + 3 * i26].im;
      }
    }
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 3; ix++) {
      for (i26 = 0; i26 < 3; i26++) {
        d = 0.0;
        s = 0.0;
        for (i = 0; i < 3; i++) {
          d += X_par_temp[i26 + 3 * i].re * dcv1[i + 3 * ix].re - X_par_temp[i26
            + 3 * i].im * dcv1[i + 3 * ix].im;
          s += X_par_temp[i26 + 3 * i].re * dcv1[i + 3 * ix].im + X_par_temp[i26
            + 3 * i].im * dcv1[i + 3 * ix].re;
        }

        c_X_par_temp[ix + 3 * i26].re = d;
        c_X_par_temp[ix + 3 * i26].im = -s;
      }
    }

    mldivide(b_X_par_temp, c_X_par_temp, dcv1);
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 3; ix++) {
      for (i26 = 0; i26 < 3; i26++) {
        LOGMX[i26 + 3 * ix].re = dcv1[ix + 3 * i26].re;
        LOGMX[i26 + 3 * ix].im = -dcv1[ix + 3 * i26].im;
      }
    }
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 9; ix++) {
      c_X_par_temp[ix].re = 0.5 * LOGMX[ix].re;
      c_X_par_temp[ix].im = 0.5 * LOGMX[ix].im;
    }

    expm(c_X_par_temp, LOGMX);
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 3; ix++) {
      for (i26 = 0; i26 < 3; i26++) {
        c_X_par_temp[i26 + 3 * ix].re = X_par[((i26 + 3 * ix) + 9 * par) + 225 *
          ((int32_T)(t - 1.0) - 1)];
        c_X_par_temp[i26 + 3 * ix].im = 0.0;
      }
    }
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 3; ix++) {
      for (i26 = 0; i26 < 3; i26++) {
        X_par_temp[ix + 3 * i26].re = 0.0;
        X_par_temp[ix + 3 * i26].im = 0.0;
        for (i = 0; i < 3; i++) {
          X_par_temp[ix + 3 * i26].re += c_X_par_temp[ix + 3 * i].re * LOGMX[i +
            3 * i26].re - 0.0 * LOGMX[i + 3 * i26].im;
          X_par_temp[ix + 3 * i26].im += c_X_par_temp[ix + 3 * i].re * LOGMX[i +
            3 * i26].im + 0.0 * LOGMX[i + 3 * i26].re;
        }
      }
    }

    b_center_x[0] = center_x;
    b_center_x[1] = center_y;
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 2; ix++) {
      for (i26 = 0; i26 < 2; i26++) {
        Aff_matrix[i26 + 3 * ix] = X_par_temp[i26 + 3 * ix].re;
      }
    }
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 2; ix++) {
      Aff_matrix[6 + ix] = X_par_temp[6 + ix].re + b_center_x[ix];
    }

//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 3; ix++) {
      Aff_matrix[2 + 3 * ix] = (real_T)iv10[ix];
    }

    /*              Aff_matrix = real([X_par_tempArray(1:2,1:2,par) X_par_tempArray(1:2,3,par)+[center_x;center_y];0 0 1]);  % AR-based state dynamics model (infinitesimal constant velocity model) eq 8,9 */
    b_image_warping(*(real_T (*)[336042])&ext_frame[0], Aff_matrix, point_matrix,
                    *(real_T (*)[1296])&warped_img[0]);
    b_image_warping(*(real_T (*)[336042])&ext_frame[336042], Aff_matrix,
                    point_matrix, *(real_T (*)[1296])&warped_img[1296]);
    b_image_warping(*(real_T (*)[336042])&ext_frame[672084], Aff_matrix,
                    point_matrix, *(real_T (*)[1296])&warped_img[2592]);

    /*             %% replace zeros with average values */
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 1296; ix++) {
      b_zero_ind_z = (warped_img[2592 + ix] == 0.0);
      zero_ind_z[ix] = !b_zero_ind_z;
      c_zero_ind_z[ix] = b_zero_ind_z;
    }

    eml_li_find(c_zero_ind_z, tmp_data, tmp_size);
    eml_li_find(zero_ind_z, b_tmp_data, b_tmp_size);
    warped_img_size[0] = b_tmp_size[0];
    i = b_tmp_size[0] - 1;
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix <= i; ix++) {
      warped_grad_x3[ix] = warped_img[b_tmp_data[ix] + 2591];
    }

    d = mean(warped_grad_x3, warped_img_size);
    i = tmp_size[0] - 1;
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix <= i; ix++) {
      warped_img[tmp_data[ix] + 2591] = d;
    }

    eml_li_find(c_zero_ind_z, tmp_data, tmp_size);
//#pragma omp for schedule (dynamic, 8)
    for (i = 0; i < 1296; i++) {
      zero_ind_z[i] = !c_zero_ind_z[i];
    }

    eml_li_find(zero_ind_z, b_tmp_data, b_tmp_size);
    b_warped_img_size[0] = b_tmp_size[0];
    i = b_tmp_size[0] - 1;
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix <= i; ix++) {
      warped_grad_x3[ix] = warped_img[b_tmp_data[ix] - 1];
    }

    d = mean(warped_grad_x3, b_warped_img_size);
    i = tmp_size[0] - 1;
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix <= i; ix++) {
      warped_img[tmp_data[ix] - 1] = d;
    }

    eml_li_find(c_zero_ind_z, tmp_data, tmp_size);
//#pragma omp for schedule (dynamic, 8)
    for (i = 0; i < 1296; i++) {
      zero_ind_z[i] = !c_zero_ind_z[i];
    }

    eml_li_find(zero_ind_z, b_tmp_data, b_tmp_size);
    c_warped_img_size[0] = b_tmp_size[0];
    i = b_tmp_size[0] - 1;
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix <= i; ix++) {
      warped_grad_x3[ix] = warped_img[b_tmp_data[ix] + 1295];
    }

    d = mean(warped_grad_x3, c_warped_img_size);
    i = tmp_size[0] - 1;
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix <= i; ix++) {
      warped_img[tmp_data[ix] + 1295] = d;
    }

    /*             %% */
    b_image_warping(ext_grad_x, Aff_matrix, point_matrix, warped_grad_x3);
    b_image_warping(ext_grad_y, Aff_matrix, point_matrix, warped_grad_y3);
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 1296; ix++) {
      img_grad[ix] = warped_grad_x3[ix];
      img_grad[1296 + ix] = warped_grad_y3[ix];
    }

    /*             %% compute rigid transform */
    estimateRigidTransform(mean_img, warped_img, transform);
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 1296; ix++) {
      for (i26 = 0; i26 < 3; i26++) {
        b_warped_img[i26 + (ix << 2)] = warped_img[ix + 1296 * i26];
      }

      b_warped_img[3 + (ix << 2)] = 1.0;
//#pragma omp for schedule (dynamic, 8)
      for (i26 = 0; i26 < 4; i26++) {
        b_transform[ix + 1296 * i26] = 0.0;
        for (i = 0; i < 4; i++) {
          b_transform[ix + 1296 * i26] += transform[i26 + (i << 2)] *
            b_warped_img[i + (ix << 2)];
        }
      }
    }
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 1296; ix++) {
      warped_grad_x3[ix] = b_transform[2592 + ix] - mean_img[2592 + ix];
    }
//#pragma omp for schedule (dynamic, 8)
    for (i = 0; i < 1296; i++) {
      d = 2.0 * warped_grad_x3[i];
      c_a[i] = d * img_grad[i];
      c_a[1296 + i] = d * img_grad[1296 + i];
      b_a[i] = c_a[i] * dw_dp[12 * i] + c_a[1296 + i] * dw_dp[1 + 12 * i];
      b_a[1296 + i] = c_a[i] * dw_dp[2 + 12 * i] + c_a[1296 + i] * dw_dp[3 + 12 *
        i];
      b_a[2592 + i] = c_a[i] * dw_dp[4 + 12 * i] + c_a[1296 + i] * dw_dp[5 + 12 *
        i];
      b_a[3888 + i] = c_a[i] * dw_dp[6 + 12 * i] + c_a[1296 + i] * dw_dp[7 + 12 *
        i];
      b_a[5184 + i] = c_a[i] * dw_dp[8 + 12 * i] + c_a[1296 + i] * dw_dp[9 + 12 *
        i];
      b_a[6480 + i] = c_a[i] * dw_dp[10 + 12 * i] + c_a[1296 + i] * dw_dp[11 +
        12 * i];
    }

    sum_jacobian[0] = X_par_temp[0];
    sum_jacobian[1] = X_par_temp[1];
    sum_jacobian[2] = X_par_temp[3];
    sum_jacobian[3] = X_par_temp[4];
    sum_jacobian[4].re = 0.0;
    sum_jacobian[4].im = 0.0;
    sum_jacobian[5].re = 0.0;
    sum_jacobian[5].im = 0.0;
    increment[0] = X_par_temp[0];
    increment[1] = X_par_temp[1];
    increment[2].re = -X_par_temp[3].re;
    increment[2].im = -X_par_temp[3].im;
    increment[3].re = -X_par_temp[4].re;
    increment[3].im = -X_par_temp[4].im;
    increment[4].re = 0.0;
    increment[4].im = 0.0;
    increment[5].re = 0.0;
    increment[5].im = 0.0;
    d_X_par_temp[0] = X_par_temp[3];
    d_X_par_temp[1] = X_par_temp[4];
    d_X_par_temp[2].re = -X_par_temp[0].re;
    d_X_par_temp[2].im = -X_par_temp[0].im;
    d_X_par_temp[3].re = -X_par_temp[1].re;
    d_X_par_temp[3].im = -X_par_temp[1].im;
    d_X_par_temp[4].re = 0.0;
    d_X_par_temp[4].im = 0.0;
    d_X_par_temp[5].re = 0.0;
    d_X_par_temp[5].im = 0.0;
    e_X_par_temp[0] = X_par_temp[3];
    e_X_par_temp[1] = X_par_temp[4];
    e_X_par_temp[2] = X_par_temp[0];
    e_X_par_temp[3] = X_par_temp[1];
    e_X_par_temp[4].re = 0.0;
    e_X_par_temp[4].im = 0.0;
    e_X_par_temp[5].re = 0.0;
    e_X_par_temp[5].im = 0.0;
    dcv2[0].re = 0.0;
    dcv2[0].im = 0.0;
    dcv2[1].re = 0.0;
    dcv2[1].im = 0.0;
    dcv2[2].re = 0.0;
    dcv2[2].im = 0.0;
    dcv2[3].re = 0.0;
    dcv2[3].im = 0.0;
    dcv2[4] = X_par_temp[0];
    dcv2[5] = X_par_temp[1];
    dcv3[0].re = 0.0;
    dcv3[0].im = 0.0;
    dcv3[1].re = 0.0;
    dcv3[1].im = 0.0;
    dcv3[2].re = 0.0;
    dcv3[2].im = 0.0;
    dcv3[3].re = 0.0;
    dcv3[3].im = 0.0;
    dcv3[4] = X_par_temp[3];
    dcv3[5] = X_par_temp[4];
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 6; ix++) {
      for (i26 = 0; i26 < 1296; i26++) {
        d_a[i26 + 1296 * ix].re = b_a[i26 + 1296 * ix];
        d_a[i26 + 1296 * ix].im = 0.0;
      }

      f_X_par_temp[ix] = sum_jacobian[ix];
      f_X_par_temp[6 + ix] = increment[ix];
      f_X_par_temp[12 + ix] = d_X_par_temp[ix];
      f_X_par_temp[18 + ix] = e_X_par_temp[ix];
      f_X_par_temp[24 + ix] = dcv2[ix];
      f_X_par_temp[30 + ix] = dcv3[ix];
    }
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 1296; ix++) {
      for (i26 = 0; i26 < 6; i26++) {
        a[ix + 1296 * i26].re = 0.0;
        a[ix + 1296 * i26].im = 0.0;
        for (i = 0; i < 6; i++) {
          a[ix + 1296 * i26].re += d_a[ix + 1296 * i].re * f_X_par_temp[i + 6 *
            i26].re - 0.0 * f_X_par_temp[i + 6 * i26].im;
          a[ix + 1296 * i26].im += d_a[ix + 1296 * i].re * f_X_par_temp[i + 6 *
            i26].im + 0.0 * f_X_par_temp[i + 6 * i26].re;
        }
      }
    }

    b_sum(a, sum_jacobian);
//#pragma omp for schedule (dynamic, 8)
    /* eq 17 -> P */
    for (ix = 0; ix < 6; ix++) {
      for (i26 = 0; i26 < 6; i26++) {
        f_X_par_temp[i26 + 6 * ix].re = dv13[i26 + 6 * ix];
        f_X_par_temp[i26 + 6 * ix].im = 0.0;
      }
    }
//#pragma omp for schedule (dynamic, 8)
    /* eq 17 -> P*J' */
    for (ix = 0; ix < 6; ix++) {
      sigma_12[ix].re = 0.0;
      sigma_12[ix].im = 0.0;
      for (i26 = 0; i26 < 6; i26++) {
        sigma_12[ix].re += f_X_par_temp[ix + 6 * i26].re * sum_jacobian[i26].re
          - 0.0 * -sum_jacobian[i26].im;
        sigma_12[ix].im += f_X_par_temp[ix + 6 * i26].re * -sum_jacobian[i26].im
          + 0.0 * sum_jacobian[i26].re;
      }
//#pragma omp for schedule (dynamic, 8)
      for (i26 = 0; i26 < 6; i26++) {
        dcv4[i26 + 6 * ix].re = dv13[i26 + 6 * ix];
        dcv4[i26 + 6 * ix].im = 0.0;
      }

      y[ix].re = 0.0;
      y[ix].im = 0.0;
//#pragma omp for schedule (dynamic, 8)
      for (i26 = 0; i26 < 6; i26++) {
        y[ix].re += sum_jacobian[i26].re * dcv4[i26 + 6 * ix].re -
          sum_jacobian[i26].im * 0.0;
        y[ix].im += sum_jacobian[i26].re * 0.0 + sum_jacobian[i26].im * dcv4[i26
          + 6 * ix].re;
      }

      increment[ix].re = sum_jacobian[ix].re;
      increment[ix].im = -sum_jacobian[ix].im;
    }

    sigma_22_re = 0.0;
    sigma_22_im = 0.0;
//#pragma omp for schedule (dynamic, 8)
    for (i = 0; i < 6; i++) {
      sigma_22_re += y[i].re * increment[i].re - y[i].im * increment[i].im;
      sigma_22_im += y[i].re * increment[i].im + y[i].im * increment[i].re;
    }

    sigma_22_re++;

    /* eq 17 -> J*P*J'+R */
    b = -norm(warped_grad_x3);
//#pragma omp for schedule (dynamic, 8)
    for (i = 0; i < 6; i++) {
      if (sigma_22_im == 0.0) {
        if (sigma_12[i].im == 0.0) {
          sigma_12_re = sigma_12[i].re / sigma_22_re;
          d = 0.0;
        } else if (sigma_12[i].re == 0.0) {
          sigma_12_re = 0.0;
          d = sigma_12[i].im / sigma_22_re;
        } else {
          sigma_12_re = sigma_12[i].re / sigma_22_re;
          d = sigma_12[i].im / sigma_22_re;
        }
      } else if (sigma_22_re == 0.0) {
        if (sigma_12[i].re == 0.0) {
          sigma_12_re = sigma_12[i].im / sigma_22_im;
          d = 0.0;
        } else if (sigma_12[i].im == 0.0) {
          sigma_12_re = 0.0;
          d = -(sigma_12[i].re / sigma_22_im);
        } else {
          sigma_12_re = sigma_12[i].im / sigma_22_im;
          d = -(sigma_12[i].re / sigma_22_im);
        }
      } else {
        brm = fabs(sigma_22_re);
        d = fabs(sigma_22_im);
        if (brm > d) {
          s = sigma_22_im / sigma_22_re;
          d = sigma_22_re + s * sigma_22_im;
          sigma_12_re = (sigma_12[i].re + s * sigma_12[i].im) / d;
          d = (sigma_12[i].im - s * sigma_12[i].re) / d;
        } else if (d == brm) {
          d = sigma_22_re > 0.0 ? 0.5 : -0.5;
          s = sigma_22_im > 0.0 ? 0.5 : -0.5;
          sigma_12_re = (sigma_12[i].re * d + sigma_12[i].im * s) / brm;
          d = (sigma_12[i].im * d - sigma_12[i].re * s) / brm;
        } else {
          s = sigma_22_re / sigma_22_im;
          d = sigma_22_im + s * sigma_22_re;
          sigma_12_re = (s * sigma_12[i].re + sigma_12[i].im) / d;
          d = (s * sigma_12[i].im - sigma_12[i].re) / d;
        }
      }

      increment[i].re = b * sigma_12_re;
      increment[i].im = b * d;
    }
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 9; ix++) {
      c_X_par_temp[ix].re = ((((increment[0].re * (real_T)g_b[ix] + increment[1]
        .re * (real_T)f_b[ix]) + increment[2].re * (real_T)e_b[ix]) + increment
        [3].re * (real_T)d_b[ix]) + increment[4].re * (real_T)c_b[ix]) +
        increment[5].re * (real_T)b_b[ix];
      c_X_par_temp[ix].im = ((((increment[0].im * (real_T)g_b[ix] + increment[1]
        .im * (real_T)f_b[ix]) + increment[2].im * (real_T)e_b[ix]) + increment
        [3].im * (real_T)d_b[ix]) + increment[4].im * (real_T)c_b[ix]) +
        increment[5].im * (real_T)b_b[ix];
    }

    expm(c_X_par_temp, LOGMX);
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 6; ix++) {
      if (sigma_22_im == 0.0) {
        if (sigma_12[ix].im == 0.0) {
          sum_jacobian[ix].re = sigma_12[ix].re / sigma_22_re;
          sum_jacobian[ix].im = 0.0;
        } else if (sigma_12[ix].re == 0.0) {
          sum_jacobian[ix].re = 0.0;
          sum_jacobian[ix].im = sigma_12[ix].im / sigma_22_re;
        } else {
          sum_jacobian[ix].re = sigma_12[ix].re / sigma_22_re;
          sum_jacobian[ix].im = sigma_12[ix].im / sigma_22_re;
        }
      } else if (sigma_22_re == 0.0) {
        if (sigma_12[ix].re == 0.0) {
          sum_jacobian[ix].re = sigma_12[ix].im / sigma_22_im;
          sum_jacobian[ix].im = 0.0;
        } else if (sigma_12[ix].im == 0.0) {
          sum_jacobian[ix].re = 0.0;
          sum_jacobian[ix].im = -(sigma_12[ix].re / sigma_22_im);
        } else {
          sum_jacobian[ix].re = sigma_12[ix].im / sigma_22_im;
          sum_jacobian[ix].im = -(sigma_12[ix].re / sigma_22_im);
        }
      } else {
        brm = fabs(sigma_22_re);
        d = fabs(sigma_22_im);
        if (brm > d) {
          s = sigma_22_im / sigma_22_re;
          d = sigma_22_re + s * sigma_22_im;
          sum_jacobian[ix].re = (sigma_12[ix].re + s * sigma_12[ix].im) / d;
          sum_jacobian[ix].im = (sigma_12[ix].im - s * sigma_12[ix].re) / d;
        } else if (d == brm) {
          d = sigma_22_re > 0.0 ? 0.5 : -0.5;
          s = sigma_22_im > 0.0 ? 0.5 : -0.5;
          sum_jacobian[ix].re = (sigma_12[ix].re * d + sigma_12[ix].im * s) /
            brm;
          sum_jacobian[ix].im = (sigma_12[ix].im * d - sigma_12[ix].re * s) /
            brm;
        } else {
          s = sigma_22_re / sigma_22_im;
          d = sigma_22_im + s * sigma_22_re;
          sum_jacobian[ix].re = (s * sigma_12[ix].re + sigma_12[ix].im) / d;
          sum_jacobian[ix].im = (s * sigma_12[ix].im - sigma_12[ix].re) / d;
        }
      }
    }
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 6; ix++) {
      for (i26 = 0; i26 < 6; i26++) {
        CH[i26 + 6 * ix] = dv13[i26 + 6 * ix] - (sum_jacobian[i26].re *
          sigma_12[ix].re - sum_jacobian[i26].im * -sigma_12[ix].im);
      }
    }

    chol(CH);
    randn(h_b);
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 6; ix++) {
      random_coeff[ix] = 0.0;
      for (i26 = 0; i26 < 6; i26++) {
        random_coeff[ix] += CH[i26 + 6 * ix] * h_b[i26];
      }
    }
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 9; ix++) {
      b_random_coeff[ix] = ((((random_coeff[0] * (real_T)g_b[ix] / 2.0 +
        random_coeff[1] * (real_T)f_b[ix]) + random_coeff[2] * (real_T)e_b[ix])
        + random_coeff[3] * (real_T)d_b[ix]) + random_coeff[4] * (real_T)c_b[ix])
        + random_coeff[5] * (real_T)b_b[ix];
    }

    b_expm(b_random_coeff, i_b);
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 3; ix++) {
      for (i26 = 0; i26 < 3; i26++) {
        b_X_par_temp[ix + 3 * i26].re = 0.0;
        b_X_par_temp[ix + 3 * i26].im = 0.0;
        for (i = 0; i < 3; i++) {
          b_X_par_temp[ix + 3 * i26].re += X_par_temp[ix + 3 * i].re * LOGMX[i +
            3 * i26].re - X_par_temp[ix + 3 * i].im * LOGMX[i + 3 * i26].im;
          b_X_par_temp[ix + 3 * i26].im += X_par_temp[ix + 3 * i].re * LOGMX[i +
            3 * i26].im + X_par_temp[ix + 3 * i].im * LOGMX[i + 3 * i26].re;
        }

        c_X_par_temp[i26 + 3 * ix].re = i_b[i26 + 3 * ix];
        c_X_par_temp[i26 + 3 * ix].im = 0.0;
      }
    }
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 3; ix++) {
      for (i26 = 0; i26 < 3; i26++) {
        d = 0.0;
        for (i = 0; i < 3; i++) {
          d += b_X_par_temp[ix + 3 * i].re * c_X_par_temp[i + 3 * i26].re -
            b_X_par_temp[ix + 3 * i].im * 0.0;
        }

        X_par_pred[((ix + 3 * i26) + 9 * par) + 225 * ((int32_T)t - 1)] = d;
      }
    }

    b_mldivide(*(real_T (*)[9])&X_par[9 * par + 225 * ((int32_T)(t - 1.0) - 1)],
               *(real_T (*)[9])&X_par_pred[9 * par + 225 * ((int32_T)t - 1)],
               b_random_coeff);
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 3; ix++) {
      for (i26 = 0; i26 < 3; i26++) {
        AR_velocity[((i26 + 3 * ix) + 9 * par) + 225 * ((int32_T)t - 1)] =
          b_random_coeff[i26 + 3 * ix];
      }
    }

    b_center_x[0] = center_x;
    b_center_x[1] = center_y;
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 2; ix++) {
      for (i26 = 0; i26 < 2; i26++) {
        Aff_matrix[i26 + 3 * ix] = X_par_pred[((i26 + 3 * ix) + 9 * par) + 225 *
          ((int32_T)t - 1)];
      }
    }
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 2; ix++) {
      Aff_matrix[6 + ix] = X_par_pred[6 + ((ix + 9 * par) + 225 * ((int32_T)t -
        1))] + b_center_x[ix];
    }
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 3; ix++) {
      Aff_matrix[2 + 3 * ix] = (real_T)iv10[ix];
    }

    b_image_warping(*(real_T (*)[336042])&ext_frame[0], Aff_matrix, point_matrix,
                    *(real_T (*)[1296])&warped_img[0]);
    b_image_warping(*(real_T (*)[336042])&ext_frame[336042], Aff_matrix,
                    point_matrix, *(real_T (*)[1296])&warped_img[1296]);
    b_image_warping(*(real_T (*)[336042])&ext_frame[672084], Aff_matrix,
                    point_matrix, *(real_T (*)[1296])&warped_img[2592]);

    /*             %% replace zeros with average values */
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 1296; ix++) {
      b_zero_ind_z = (warped_img[2592 + ix] == 0.0);
      zero_ind_z[ix] = !b_zero_ind_z;
      c_zero_ind_z[ix] = b_zero_ind_z;
    }

    eml_li_find(c_zero_ind_z, tmp_data, tmp_size);
    eml_li_find(zero_ind_z, b_tmp_data, b_tmp_size);
    d_warped_img_size[0] = b_tmp_size[0];
    i = b_tmp_size[0] - 1;
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix <= i; ix++) {
      warped_grad_x3[ix] = warped_img[b_tmp_data[ix] + 2591];
    }

    d = mean(warped_grad_x3, d_warped_img_size);
    i = tmp_size[0] - 1;
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix <= i; ix++) {
      warped_img[tmp_data[ix] + 2591] = d;
    }

    eml_li_find(c_zero_ind_z, tmp_data, tmp_size);
    for (i = 0; i < 1296; i++) {
      zero_ind_z[i] = !c_zero_ind_z[i];
    }

    eml_li_find(zero_ind_z, b_tmp_data, b_tmp_size);
    e_warped_img_size[0] = b_tmp_size[0];
    i = b_tmp_size[0] - 1;
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix <= i; ix++) {
      warped_grad_x3[ix] = warped_img[b_tmp_data[ix] - 1];
    }

    d = mean(warped_grad_x3, e_warped_img_size);
    i = tmp_size[0] - 1;
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix <= i; ix++) {
      warped_img[tmp_data[ix] - 1] = d;
    }

    eml_li_find(c_zero_ind_z, tmp_data, tmp_size);
//#pragma omp for schedule (dynamic, 8)
    for (i = 0; i < 1296; i++) {
      zero_ind_z[i] = !c_zero_ind_z[i];
    }

    eml_li_find(zero_ind_z, b_tmp_data, b_tmp_size);
    f_warped_img_size[0] = b_tmp_size[0];
    i = b_tmp_size[0] - 1;
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix <= i; ix++) {
      warped_grad_x3[ix] = warped_img[b_tmp_data[ix] + 1295];
    }

    d = mean(warped_grad_x3, f_warped_img_size);
    i = tmp_size[0] - 1;
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix <= i; ix++) {
      warped_img[tmp_data[ix] + 1295] = d;
    }

    /*             %% compute rigid transform */
    estimateRigidTransform(mean_img, warped_img, transform);
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 4; ix++) {
      for (i26 = 0; i26 < 4; i26++) {
        rigid_transform_par[(i26 + (ix << 2)) + (par << 4)] = transform[i26 +
          (ix << 2)];
      }
    }
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 1296; ix++) {
      for (i26 = 0; i26 < 3; i26++) {
        b_warped_img[i26 + (ix << 2)] = warped_img[ix + 1296 * i26];
      }

      b_warped_img[3 + (ix << 2)] = 1.0;
//#pragma omp for schedule (dynamic, 8)
      for (i26 = 0; i26 < 4; i26++) {
        c_transform[ix + 1296 * i26] = 0.0;
        for (i = 0; i < 4; i++) {
          c_transform[ix + 1296 * i26] += transform[i26 + (i << 2)] *
            b_warped_img[i + (ix << 2)];
        }
      }
    }
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 3; ix++) {
      memcpy(&warped_img_tr[1296 * ix], &c_transform[1296 * ix], 1296U * sizeof
             (real_T));
    }
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 1296; ix++) {
      b_warped_img_tr[ix] = warped_img_tr[2592 + ix] - mean_img[2592 + ix];
    }
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 1296; ix++) {
      b_warped_img_tr[ix + 1296] = warped_img_tr[1296 + ix] - mean_img[1296 + ix];
    }
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 1296; ix++) {
      b_warped_img_tr[ix + 2592] = warped_img_tr[ix] - mean_img[ix];
    }

    d = b_norm(b_warped_img_tr) / 3.0;

    /*             %% */
    dist1Array[par] = d * d;
  }
//#pragma omp for schedule (dynamic, 8)
  for (i = 0; i < 25; i++) {
    b_dist1Array[i] = (dist1Array[i] > 1000.0);
  }

  b_eml_li_find(b_dist1Array, c_tmp_data, tmp_size);
  i = tmp_size[0] - 1;
//#pragma omp for schedule (dynamic, 8)
  for (ix = 0; ix <= i; ix++) {
    dist1Array[c_tmp_data[ix] - 1] = 1000.0;
  }
//#pragma omp for schedule (dynamic, 8)
  for (ix = 0; ix < 25; ix++) {
    dist1Array[ix] *= -0.5;
  }

  b_exp(dist1Array);

  /* % Particle resampling */
  d = c_sum(dist1Array);
//#pragma omp for schedule (dynamic, 8)
  for (ix = 0; ix < 25; ix++) {
    dist1Array[ix] /= d;
  }

  resampling(dist1Array, outindex);
//#pragma omp for schedule (dynamic, 8)
  for (ix = 0; ix < 25; ix++) {
    for (i26 = 0; i26 < 3; i26++) {
      for (i = 0; i < 3; i++) {
        b_X_par_pred[(i + 3 * i26) + 9 * ix] = X_par_pred[((i + 3 * i26) + 9 *
          ((int32_T)outindex[ix] - 1)) + 225 * ((int32_T)t - 1)];
      }
    }
  }
//#pragma omp for schedule (dynamic, 8)
  for (ix = 0; ix < 25; ix++) {
    for (i26 = 0; i26 < 3; i26++) {
      for (i = 0; i < 3; i++) {
        X_par[((i + 3 * i26) + 9 * ix) + 225 * ((int32_T)t - 1)] = b_X_par_pred
          [(i + 3 * i26) + 9 * ix];
      }
    }
  }
//#pragma omp for schedule (dynamic, 8)
  for (ix = 0; ix < 25; ix++) {
    for (i26 = 0; i26 < 3; i26++) {
      for (i = 0; i < 3; i++) {
        b_X_par_pred[(i + 3 * i26) + 9 * ix] = AR_velocity[((i + 3 * i26) + 9 *
          ((int32_T)outindex[ix] - 1)) + 225 * ((int32_T)t - 1)];
      }
    }
  }
//#pragma omp for schedule (dynamic, 8)
  for (ix = 0; ix < 25; ix++) {
    for (i26 = 0; i26 < 3; i26++) {
      for (i = 0; i < 3; i++) {
        AR_velocity[((i + 3 * i26) + 9 * ix) + 225 * ((int32_T)t - 1)] =
          b_X_par_pred[(i + 3 * i26) + 9 * ix];
      }
    }
  }
//#pragma omp for schedule (dynamic, 8)
  for (ix = 0; ix < 25; ix++) {
    for (i26 = 0; i26 < 4; i26++) {
      for (i = 0; i < 4; i++) {
        b_rigid_transform_par[(i + (i26 << 2)) + (ix << 4)] =
          rigid_transform_par[(i + (i26 << 2)) + (((int32_T)outindex[ix] - 1) <<
          4)];
      }
    }
  }
//#pragma omp for schedule (dynamic, 8)
  for (ix = 0; ix < 25; ix++) {
    for (i26 = 0; i26 < 4; i26++) {
      for (i = 0; i < 4; i++) {
        rigid_transform_par[(i + (i26 << 2)) + (ix << 4)] =
          b_rigid_transform_par[(i + (i26 << 2)) + (ix << 4)];
      }
    }
  }

  /* % Computing affine state particle mean  */
  i = 1;
  d = dist1Array[0];
  itmp = 0;
  if (rtIsNaN(dist1Array[0])) {
    ix = 2;
    exitg1 = FALSE;
    while ((exitg1 == 0U) && (ix < 26)) {
      i = ix;
      if (!rtIsNaN(dist1Array[ix - 1])) {
        d = dist1Array[ix - 1];
        exitg1 = TRUE;
      } else {
        ix++;
      }
    }
  }

  if (i < 25) {
    while (i + 1 < 26) {
      if (dist1Array[i] > d) {
        d = dist1Array[i];
        itmp = i;
      }

      i++;
    }
  }
//#pragma omp for schedule (dynamic, 8)
  for (ix = 0; ix < 4; ix++) {
    mean_log[ix] = 0.0;
  }
//#pragma omp for schedule (dynamic, 8)
  for (par = 0; par < 25; par++) {
    for (ix = 0; ix < 2; ix++) {
      for (i26 = 0; i26 < 2; i26++) {
        c_X_par_pred[i26 + (ix << 1)] = X_par_pred[((i26 + 3 * ix) + 9 * itmp) +
          225 * ((int32_T)t - 1)];
      }
    }
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 2; ix++) {
      for (i26 = 0; i26 < 2; i26++) {
        b_X_par[i26 + (ix << 1)] = X_par[((i26 + 3 * ix) + 9 * par) + 225 *
          ((int32_T)t - 1)];
      }
    }

    c_mldivide(c_X_par_pred, b_X_par, b_LOGMX);
    b_eig(b_LOGMX, V, D);
    for (ix = 0; ix < 2; ix++) {
      dcv5[ix] = D[3 * ix];
    }

    c_log(dcv5);
    b_diag(dcv5, D);
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 2; ix++) {
      for (i26 = 0; i26 < 2; i26++) {
        b_V[ix + (i26 << 1)].re = 0.0;
        b_V[ix + (i26 << 1)].im = 0.0;
        for (i = 0; i < 2; i++) {
          b_V[ix + (i26 << 1)].re += V[ix + (i << 1)].re * D[i + (i26 << 1)].re
            - V[ix + (i << 1)].im * D[i + (i26 << 1)].im;
          b_V[ix + (i26 << 1)].im += V[ix + (i << 1)].re * D[i + (i26 << 1)].im
            + V[ix + (i << 1)].im * D[i + (i26 << 1)].re;
        }
      }
    }

    b_mrdivide(b_V, V, D);
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 4; ix++) {
      mean_log[ix] += D[ix].re / 25.0;
    }
  }

  c_expm(mean_log, b_LOGMX);
//#pragma omp for schedule (dynamic, 8)
  for (ix = 0; ix < 25; ix++) {
    for (i26 = 0; i26 < 2; i26++) {
      c_X_par[i26 + (ix << 1)] = X_par[6 + ((i26 + 9 * ix) + 225 * ((int32_T)t -
        1))];
    }
  }

  b_mean(c_X_par, mean_trans);

  /*  mean_X_par(:,:,t) = [mean_gl mean_trans;0 0 1]; */
  /* % Tracked object image update */
  b_center_x[0] = center_x;
  b_center_x[1] = center_y;
//#pragma omp for schedule (dynamic, 8)
  for (ix = 0; ix < 2; ix++) {
    for (i26 = 0; i26 < 2; i26++) {
      c_X_par_pred[ix + (i26 << 1)] = 0.0;
      for (i = 0; i < 2; i++) {
        c_X_par_pred[ix + (i26 << 1)] += X_par_pred[((ix + 3 * i) + 9 * itmp) +
          225 * ((int32_T)t - 1)] * b_LOGMX[i + (i26 << 1)];
      }
    }
  }
//#pragma omp for schedule (dynamic, 8)
  for (ix = 0; ix < 2; ix++) {
    for (i26 = 0; i26 < 2; i26++) {
      Aff_matrix[i26 + 3 * ix] = c_X_par_pred[i26 + (ix << 1)];
    }
  }
//#pragma omp for schedule (dynamic, 8)
  for (ix = 0; ix < 2; ix++) {
    Aff_matrix[6 + ix] = mean_trans[ix] + b_center_x[ix];
  }
//#pragma omp for schedule (dynamic, 8)
  for (ix = 0; ix < 3; ix++) {
    Aff_matrix[2 + 3 * ix] = (real_T)iv10[ix];
  }

  /*  %% apply rigid transform to the tracked image and replace zeros with average values */
  memset(&warped_img[0], 0, 3888U * sizeof(real_T));
  b_image_warping(*(real_T (*)[336042])&ext_frame[0], Aff_matrix, point_matrix, *
                  (real_T (*)[1296])&warped_img[0]);
  b_image_warping(*(real_T (*)[336042])&ext_frame[336042], Aff_matrix,
                  point_matrix, *(real_T (*)[1296])&warped_img[1296]);
  b_image_warping(*(real_T (*)[336042])&ext_frame[672084], Aff_matrix,
                  point_matrix, *(real_T (*)[1296])&warped_img[2592]);
//#pragma omp for schedule (dynamic, 8)
  for (ix = 0; ix < 1296; ix++) {
    b_zero_ind_z = (warped_img[2592 + ix] == 0.0);
    zero_ind_z[ix] = !b_zero_ind_z;
    c_zero_ind_z[ix] = b_zero_ind_z;
  }

  eml_li_find(c_zero_ind_z, tmp_data, tmp_size);
  eml_li_find(zero_ind_z, b_tmp_data, b_tmp_size);
  g_warped_img_size[0] = b_tmp_size[0];
  i = b_tmp_size[0] - 1;
//#pragma omp for schedule (dynamic, 8)
  for (ix = 0; ix <= i; ix++) {
    warped_grad_x3[ix] = warped_img[b_tmp_data[ix] + 2591];
  }

  d = mean(warped_grad_x3, g_warped_img_size);
  i = tmp_size[0] - 1;
//#pragma omp for schedule (dynamic, 8)
  for (ix = 0; ix <= i; ix++) {
    warped_img[tmp_data[ix] + 2591] = d;
  }

  eml_li_find(c_zero_ind_z, tmp_data, tmp_size);
//#pragma omp for schedule (dynamic, 8)
  for (i = 0; i < 1296; i++) {
    zero_ind_z[i] = !c_zero_ind_z[i];
  }

  eml_li_find(zero_ind_z, b_tmp_data, b_tmp_size);
  h_warped_img_size[0] = b_tmp_size[0];
  i = b_tmp_size[0] - 1;
//#pragma omp for schedule (dynamic, 8)
  for (ix = 0; ix <= i; ix++) {
    warped_grad_x3[ix] = warped_img[b_tmp_data[ix] - 1];
  }

  d = mean(warped_grad_x3, h_warped_img_size);
  i = tmp_size[0] - 1;
//#pragma omp for schedule (dynamic, 8)
  for (ix = 0; ix <= i; ix++) {
    warped_img[tmp_data[ix] - 1] = d;
  }

  eml_li_find(c_zero_ind_z, tmp_data, tmp_size);
//#pragma omp for schedule (dynamic, 8)
  for (i = 0; i < 1296; i++) {
    zero_ind_z[i] = !c_zero_ind_z[i];
  }

  eml_li_find(zero_ind_z, b_tmp_data, b_tmp_size);
  i_warped_img_size[0] = b_tmp_size[0];
  i = b_tmp_size[0] - 1;
//#pragma omp for schedule (dynamic, 8)
  for (ix = 0; ix <= i; ix++) {
    warped_grad_x3[ix] = warped_img[b_tmp_data[ix] + 1295];
  }

  d = mean(warped_grad_x3, i_warped_img_size);
  i = tmp_size[0] - 1;
//#pragma omp for schedule (dynamic, 8)
  for (ix = 0; ix <= i; ix++) {
    warped_img[tmp_data[ix] + 1295] = d;
  }
//#pragma omp for schedule (dynamic, 8)
  for (ix = 0; ix < 1296; ix++) {
    for (i26 = 0; i26 < 3; i26++) {
      b_warped_img[i26 + (ix << 2)] = warped_img[ix + 1296 * i26];
    }

    b_warped_img[3 + (ix << 2)] = 1.0;
//#pragma omp for schedule (dynamic, 8)
    for (i26 = 0; i26 < 4; i26++) {
      c_rigid_transform_par[ix + 1296 * i26] = 0.0;
      for (i = 0; i < 4; i++) {
        c_rigid_transform_par[ix + 1296 * i26] += rigid_transform_par[(i26 + (i <<
          2)) + (itmp << 4)] * b_warped_img[i + (ix << 2)];
      }
    }
  }
//#pragma omp for schedule (dynamic, 8)
  for (ix = 0; ix < 3; ix++) {
    memcpy(&warped_img_tr[1296 * ix], &c_rigid_transform_par[1296 * ix], 1296U *
           sizeof(real_T));
  }
//#pragma omp for schedule (dynamic, 8)
  for (ix = 0; ix < 1296; ix++) {
    tracked_images[ix + 1296 * ((int32_T)t - 1)] = warped_img_tr[ix];
  }
//#pragma omp for schedule (dynamic, 8)
  for (ix = 0; ix < 1296; ix++) {
    tracked_images[12960000 + (ix + 1296 * ((int32_T)t - 1))] = warped_img_tr
      [1296 + ix];
  }
//#pragma omp for schedule (dynamic, 8)
  for (ix = 0; ix < 1296; ix++) {
    tracked_images[25920000 + (ix + 1296 * ((int32_T)t - 1))] = warped_img_tr
      [2592 + ix];
  }

  c_mean(warped_img, centroid);
  emxInit_real_T(&b_tracked_images, 2);
  emxInit_real_T(&c_tracked_images, 2);
  emxInit_real_T(&d_tracked_images, 2);
  if ((t >= 50.0) && (rt_remd_snf(t, 50.0) == 0.0)) {
    ix = d_tracked_images->size[0] * d_tracked_images->size[1];
    d_tracked_images->size[0] = 1296;
    d_tracked_images->size[1] = (int32_T)t;
    emxEnsureCapacity((emxArray__common *)d_tracked_images, ix, (int32_T)sizeof
                      (real_T));
    i = (int32_T)t - 1;
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix <= i; ix++) {
      for (i26 = 0; i26 < 1296; i26++) {
        d_tracked_images->data[i26 + d_tracked_images->size[0] * ix] =
          tracked_images[i26 + 1296 * ix];
      }
    }

    d_mean(d_tracked_images, warped_grad_x3);
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 1296; ix++) {
      mean_img[ix] = warped_grad_x3[ix];
    }

    ix = c_tracked_images->size[0] * c_tracked_images->size[1];
    c_tracked_images->size[0] = 1296;
    c_tracked_images->size[1] = (int32_T)t;
    emxEnsureCapacity((emxArray__common *)c_tracked_images, ix, (int32_T)sizeof
                      (real_T));
    i = (int32_T)t - 1;
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix <= i; ix++) {
      for (i26 = 0; i26 < 1296; i26++) {
        c_tracked_images->data[i26 + c_tracked_images->size[0] * ix] =
          tracked_images[12960000 + (i26 + 1296 * ix)];
      }
    }

    d_mean(c_tracked_images, warped_grad_x3);
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 1296; ix++) {
      mean_img[1296 + ix] = warped_grad_x3[ix];
    }

    ix = b_tracked_images->size[0] * b_tracked_images->size[1];
    b_tracked_images->size[0] = 1296;
    b_tracked_images->size[1] = (int32_T)t;
    emxEnsureCapacity((emxArray__common *)b_tracked_images, ix, (int32_T)sizeof
                      (real_T));
    i = (int32_T)t - 1;
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix <= i; ix++) {
      for (i26 = 0; i26 < 1296; i26++) {
        b_tracked_images->data[i26 + b_tracked_images->size[0] * ix] =
          tracked_images[25920000 + (i26 + 1296 * ix)];
      }
    }

    d_mean(b_tracked_images, warped_grad_x3);
//#pragma omp for schedule (dynamic, 8)
    for (ix = 0; ix < 1296; ix++) {
      mean_img[2592 + ix] = warped_grad_x3[ix];
    }

    /*         %% replace non connected values with mean values in the mean img */
    removeOutliers(mean_img);
  }

  emxFree_real_T(&d_tracked_images);
  emxFree_real_T(&c_tracked_images);
  emxFree_real_T(&b_tracked_images);
}

void eml_li_find(const boolean_T x[1296], int32_T y_data[1296], int32_T y_size[1])
{
  int32_T j;
  int32_T i;
  y_size[0] = compute_nones(x);
  j = 0;
  for (i = 0; i < 1296; i++) {
    if (x[i]) {
      y_data[j] = i + 1;
      j++;
    }
  }
}

/* End of code generation (compute_prob1.cpp) */