Пример #1
0
END_TEST

START_TEST(test_matrix_inverse_3x3) {
  u32 i, j, t;
  double A[9];
  double B[9];
  double I[9];

  seed_rng();
  /* 3x3 inverses */
  for (t = 0; t < LINALG_NUM; t++) {
    do {
      for (i = 0; i < 3; i++)
        for (j = 0; j < 3; j++)
          A[3*i + j] = mrand;
    } while (matrix_inverse(3, A, B) < 0);
    matrix_multiply(3, 3, 3, A, B, I);
    fail_unless(fabs(I[0] - 1) < LINALG_TOL,
                "Matrix differs from identity: %lf", I[0]);
    fail_unless(fabs(I[4] - 1) < LINALG_TOL,
                "Matrix differs from identity: %lf", I[4]);
    fail_unless(fabs(I[8] - 1) < LINALG_TOL,
                "Matrix differs from identity: %lf", I[8]);
  }
  for (i = 0; i < 3; i++)
    for (j = 0; j < 3; j++)
      if (j == 0)
        A[3*i + j] = 33;
      else
        A[3*i + j] = 1;
  s32 mi = matrix_inverse(3, A, B);
  fail_unless(mi < 0, "Singular matrix not detected.");
}
Пример #2
0
END_TEST

START_TEST(test_matrix_inverse_4x4) {
  u32 i, j, t;
  double A[16];
  double B[16];
  double I[16];

  seed_rng();
  /* 4x4 inverses */
  for (t = 0; t < LINALG_NUM; t++) {
    do {
      for (i = 0; i < 4; i++)
        for (j = 0; j < 4; j++)
          A[4*i + j] = mrand;
    } while (matrix_inverse(4, A, B) < 0);
    matrix_multiply(4, 4, 4, A, B, I);
    fail_unless(fabs(I[0] - 1) < LINALG_TOL,
                "Matrix differs from identity: %lf", I[0]);
    fail_unless(fabs(I[5] - 1) < LINALG_TOL,
                "Matrix differs from identity: %lf", I[5]);
    fail_unless(fabs(I[10] - 1) < LINALG_TOL,
                "Matrix differs from identity: %lf", I[10]);
    fail_unless(fabs(I[15] - 1) < LINALG_TOL,
                "Matrix differs from identity: %lf", I[15]);
  }
  for (i = 0; i < 4; i++)
    for (j = 0; j < 4; j++)
      if (j == 0)
        A[4*i + j] = 44;
      else
        A[4*i + j] = 1;
  s32 mi = matrix_inverse(4, A, B);
  fail_unless(mi < 0, "Singular matrix not detected.");
}
Пример #3
0
void
ColumnMajorMatrixTest::inverse()
{
  ColumnMajorMatrix matrix(3,3), matrix_inverse(3,3), e_val(3,3), e_vec(3,3);

  matrix(0,0) = 1.0;
  matrix(0,1) = 3.0;
  matrix(0,2) = 3.0;

  matrix(1,0) = 1.0;
  matrix(1,1) = 4.0;
  matrix(1,2) = 3.0;

  matrix(2,0) = 1.0;
  matrix(2,1) = 3.0;
  matrix(2,2) = 4.0;

  matrix.inverse(matrix_inverse);


  CPPUNIT_ASSERT( matrix_inverse(0,0) == 7.0 );
  CPPUNIT_ASSERT( matrix_inverse(0,1) == -3.0 );
  CPPUNIT_ASSERT( matrix_inverse(0,2) == -3.0 );

  CPPUNIT_ASSERT( matrix_inverse(1,0) == -1.0 );
  CPPUNIT_ASSERT( matrix_inverse(1,1) == 1.0 );
  CPPUNIT_ASSERT( matrix_inverse(1,2) == 0.0 );

  CPPUNIT_ASSERT( matrix_inverse(2,0) == -1.0 );
  CPPUNIT_ASSERT( matrix_inverse(2,1) == 0.0 );
  CPPUNIT_ASSERT( matrix_inverse(2,2) == 1.0 );


    /*matrix(0,0) = 2.0;
    matrix(0,1) = 1.0;
    matrix(0,2) = 1.0;

    matrix(1,0) = 1.0;
    matrix(1,1) = 2.0;
    matrix(1,2) = 1.0;

    matrix(2,0) = 1.0;
    matrix(2,1) = 1.0;
    matrix(2,2) = 2.0;

    matrix.eigen(e_val, e_vec);
    e_vec.inverse(matrix_inverse);

    e_val.print();
    e_vec.print();
    matrix_inverse.print();*/
}
Пример #4
0
int
main (int argc, char **argv)
{
  int i;
  int j;
  int k;
  double a[UPPERLIMIT][UPPERLIMIT];
  double eps = 1.0e-6;

  /*
   * Need UPPERLIMIT*UPPERLIMIT values to fill up the matrix.
   */
  if (argc != UPPERLIMIT*UPPERLIMIT+1)
  {
    return 1;
  }

  k = 0;
  for (i = 0; i < UPPERLIMIT; ++i)
  {
    for (j = 0; j < UPPERLIMIT; ++j)
    {
      a[i][j] = atoi (argv[k + 1]);
      k++;
    }
  }

  int val = matrix_inverse (a, UPPERLIMIT, UPPERLIMIT, eps);
    
  printf("%d", val);

  return 0;
}
Пример #5
0
bool
vtn_handle_glsl450_instruction(struct vtn_builder *b, uint32_t ext_opcode,
                               const uint32_t *w, unsigned count)
{
   switch ((enum GLSLstd450)ext_opcode) {
   case GLSLstd450Determinant: {
      struct vtn_value *val = vtn_push_value(b, w[2], vtn_value_type_ssa);
      val->ssa = rzalloc(b, struct vtn_ssa_value);
      val->ssa->type = vtn_value(b, w[1], vtn_value_type_type)->type->type;
      val->ssa->def = build_mat_det(b, vtn_ssa_value(b, w[5]));
      break;
   }

   case GLSLstd450MatrixInverse: {
      struct vtn_value *val = vtn_push_value(b, w[2], vtn_value_type_ssa);
      val->ssa = matrix_inverse(b, vtn_ssa_value(b, w[5]));
      break;
   }

   case GLSLstd450InterpolateAtCentroid:
   case GLSLstd450InterpolateAtSample:
   case GLSLstd450InterpolateAtOffset:
      handle_glsl450_interpolation(b, ext_opcode, w, count);
      break;

   default:
      handle_glsl450_alu(b, (enum GLSLstd450)ext_opcode, w, count);
   }

   return true;
}
Пример #6
0
static int
linverse(lua_State *L) {
	struct matrix *m = (struct matrix *)lua_touserdata(L, 1);
	struct matrix source = *m;
	if (matrix_inverse(&source, m)) {
		return luaL_error(L, "Invalid matrix");
	}
	lua_settop(L,1);
	return 1;
}
Пример #7
0
// autorelease function
matrix_t* matrix_pseudo_inverse(matrix_t* m)
{
    if (m->rows == m->cols) {
        return matrix_inverse(m);
    } else if (m->rows > m->cols) {
        matrix_begin();
        // (A^T A)^{-1} A^T
        matrix_t* ret = matrix_product(matrix_inverse(matrix_product(matrix_transpose(m), m)),matrix_transpose(m));
        matrix_retain(ret);
        matrix_end();

        return matrix_autorelease(ret);
    } else {
        matrix_begin();
        // A^T (A A^T)^{-1} 
        matrix_t* ret = matrix_product(matrix_transpose(m), matrix_inverse(matrix_product(m, matrix_transpose(m))));
        matrix_retain(ret);
        matrix_end();

        return matrix_autorelease(ret);
    }
}
Пример #8
0
END_TEST

START_TEST(test_matrix_inverse_5x5) {
  u32 i, j, t;
  double A[25];
  double B[25];
  double I[25];

  seed_rng();
  /* 5x5 inverses */
  for (t = 0; t < LINALG_NUM; t++) {
    do {
      for (i = 0; i < 5; i++)
        for (j = 0; j < 5; j++)
          A[5*i + j] = mrand;
    } while (matrix_inverse(5, A, B) < 0);
    matrix_multiply(5, 5, 5, A, B, I);
    fail_unless(fabs(I[0] - 1) < LINALG_TOL,
                "Matrix differs from identity: %lf", I[0]);
    fail_unless(fabs(I[6] - 1) < LINALG_TOL,
                "Matrix differs from identity: %lf", I[6]);
    fail_unless(fabs(I[12] - 1) < LINALG_TOL,
                "Matrix differs from identity: %lf", I[12]);
    fail_unless(fabs(I[18] - 1) < LINALG_TOL,
                "Matrix differs from identity: %lf", I[18]);
    fail_unless(fabs(I[24] - 1) < LINALG_TOL,
                "Matrix differs from identity: %lf", I[24]);
  }
  for (i = 0; i < 5; i++)
    for (j = 0; j < 5; j++)
      if (j == 0)
        A[5*i + j] = 55;
      else
        A[5*i + j] = 1;
  s32 mi = matrix_inverse(5, A, B);
  fail_unless(mi < 0, "Singular matrix not detected.");
}
Пример #9
0
int main(int argc, char** argv)
{
    printf("--------verify--------\n");
    verify_matrix();
    printf("----------------------\n");

    // autorelease関数をbeginとendにはさまないで使うのはNG
    // 全てのallocされた関数は
    // (1) free
    // (2) release
    // (3) begin-endの中でautorelease
    // のいずれかを行う必要がある。
    
    // 擬似逆行列演算がこれくらい簡単に記述できる。
    
    /* 3*2行列aを確保、成分ごとの代入 */
    matrix_t* a = matrix_alloc(3, 2);
    ELEMENT(a, 0, 0) = 1; ELEMENT(a, 0, 1) = 4;
    ELEMENT(a, 1, 0) = 2; ELEMENT(a, 1, 1) = 5;
    ELEMENT(a, 2, 0) = 3; ELEMENT(a, 2, 1) = 6;

    /* 擬似逆行列の演算 */
    // auto releaseモードに入る
    matrix_begin(); 
    // 擬似逆行列を求める。一行だけ!
    matrix_t* inva = matrix_product(matrix_inverse(matrix_product(matrix_transpose(a), a)), matrix_transpose(a));
    // 擬似逆行列の表示 (invaはmatrix_endで解放されるので、begin-end内で。)
    matrix_print(inva);
    // release poolを開放する
    matrix_end();

    /* ちなみに擬似逆行列を求める関数は内部に組んだので、それを使うこともできる。 */
    // auto releaseモードに入る
    matrix_begin(); 
    // 擬似逆行列を求める関数を呼ぶ。
    matrix_t* inva_simple = matrix_pseudo_inverse(a);
    // 擬似逆行列の表示 (invaはmatrix_endで解放されるので、begin-end内で。)
    matrix_print(inva_simple);
    // release poolを開放する
    matrix_end();

    
    // これはautorelease対象でないので、しっかり自分でfree。
    // リテインカウントを実装してあるので、理解できればreleaseの方が高性能。
    //matrix_free(a);
    matrix_release(a);
 
    return 0;
}
Пример #10
0
void clock_est_update(clock_est_state_t *s, gps_time_t meas_gpst,
                      double meas_clock_period, double localt, double q,
                      double r_gpst, double r_clock_period)
{
  double temp[2][2];

  double phi_t_0[2][2] = {{1, localt}, {0, 1}};
  double phi_t_0_tr[2][2];
  matrix_transpose(2, 2, (const double *)phi_t_0, (double *)phi_t_0_tr);

  double P_[2][2];
  memcpy(P_, s->P, sizeof(P_));
  P_[0][0] += q;

  double y[2];
  gps_time_t pred_gpst = s->t0_gps;
  pred_gpst.tow += localt * s->clock_period;
  pred_gpst = normalize_gps_time(pred_gpst);
  y[0] = gpsdifftime(meas_gpst, pred_gpst);
  y[1] = meas_clock_period - s->clock_period;

  double S[2][2];
  matrix_multiply(2, 2, 2, (const double *)phi_t_0, (const double *)P_, (double *)temp);
  matrix_multiply(2, 2, 2, (const double *)temp, (const double *)phi_t_0_tr, (double *)S);
  S[0][0] += r_gpst;
  S[1][1] += r_clock_period;
  double Sinv[2][2];
  matrix_inverse(2, (const double *)S, (double *)Sinv);

  double K[2][2];
  matrix_multiply(2, 2, 2, (const double *)P_, (const double *)phi_t_0_tr, (double *)temp);
  matrix_multiply(2, 2, 2, (const double *)temp, (const double *)Sinv, (double *)K);

  double dx[2];
  matrix_multiply(2, 2, 1, (const double *)K, (const double *)y, (double *)dx);
  s->t0_gps.tow += dx[0];
  s->t0_gps = normalize_gps_time(s->t0_gps);
  s->clock_period += dx[1];

  matrix_multiply(2, 2, 2, (const double *)K, (const double *)phi_t_0, (double *)temp);
  temp[0][0] = 1 - temp[0][0];
  temp[0][1] = -temp[0][1];
  temp[1][1] = 1 - temp[1][1];
  temp[1][0] = -temp[1][0];
  matrix_multiply(2, 2, 2, (const double *)temp, (const double *)P_, (double *)s->P);

}
Пример #11
0
void maximization(llna_model* model, llna_ss* ss)
{
    int i, j;
    double sum;

    // mean maximization

    for (i = 0; i < model->k-1; i++)
        vset(model->mu, i, vget(ss->mu_ss, i) / ss->ndata);

    // covariance maximization

    for (i = 0; i < model->k-1; i++)
    {
        for (j = 0; j < model->k-1; j++)
        {
            mset(model->cov, i, j,
                 (1.0 / ss->ndata) *
                 (mget(ss->cov_ss, i, j) +
                  ss->ndata * vget(model->mu, i) * vget(model->mu, j) -
                  vget(ss->mu_ss, i) * vget(model->mu, j) -
                  vget(ss->mu_ss, j) * vget(model->mu, i)));
        }
    }
    if (PARAMS.cov_estimate == SHRINK)
    {
        cov_shrinkage(model->cov, ss->ndata, model->cov);
    }
    matrix_inverse(model->cov, model->inv_cov);
    model->log_det_inv_cov = log_det(model->inv_cov);

    // topic maximization

    for (i = 0; i < model->k; i++)
    {
        sum = 0;
        for (j = 0; j < model->log_beta->size2; j++)
            sum += mget(ss->beta_ss, i, j);

        if (sum == 0) sum = safe_log(sum) * model->log_beta->size2;
        else sum = safe_log(sum);

        for (j = 0; j < model->log_beta->size2; j++)
            mset(model->log_beta, i, j, safe_log(mget(ss->beta_ss, i, j)) - sum);
    }
}
Пример #12
0
// MCOORD -> prime MCOORD
void mettometp(int ii, int jj, FTYPE*ucon)
{
  int j,k;
  FTYPE r, th, X[NDIM];
  FTYPE dxdxp[NDIM][NDIM];
  FTYPE idxdxp[NDIM][NDIM];
  FTYPE tmp[NDIM];

  coord(ii, jj, CENT, X);
  bl_coord(X, &r, &th);

  dxdxprim(X, r, th, dxdxp);
  // actually gcon_func() takes inverse of first arg and puts result into second arg.
  matrix_inverse(dxdxp,idxdxp);

  /* transform ucon */
  // this is u^j = (iT)^j_k u^k, as in mettobl() above
  DLOOPA tmp[j] = 0.;
  DLOOP tmp[j] += idxdxp[j][k] * ucon[k];
  DLOOPA ucon[j] = tmp[j];
  
  // note that u_{k,BL} = u_{j,KSP} (iT)^j_k  

  // note that u_{k,KSP} = u_{j,BL} T^j_k  

  // note that u^{j,BL} =  T^j_k u^{k,KSP}   // (T) called ks2bl in grmhd-transforms.nb

  // note that u^{j,KSP} = (iT)^j_k u^{k,BL} // (iT) called bl2ks in grmhd-transforms.nb

  // So T=BL2KSP for covariant components and KSP2BL for contravariant components
  // and (iT)=BL2KSP for contra and KSP2BL for cov

  // where here T=dxdxp and (iT)=idxdxp (not transposed, just inverse)

  /* done! */
}
Пример #13
0
static int
test_child(struct sprite *s, struct srt *srt, struct matrix * ts, int x, int y, struct sprite ** touch) {
	struct matrix temp;
	struct matrix *t = mat_mul(s->t.mat, ts, &temp);
	if (s->type == TYPE_ANIMATION) {
		struct sprite *tmp = NULL;
		int testin = test_animation(s , srt, t, x,y, &tmp);
		if (testin) {
			*touch = tmp;
			return 1;
		} else if (tmp) {
			if (s->message) {
				*touch = s;
				return 1;
			} else {
				*touch = tmp;
				return 0;
			}
		}
	}
	struct matrix mat;
	if (t == NULL) {
		matrix_identity(&mat);
	} else {
		mat = *t;
	}
	matrix_srt(&mat, srt);
	struct matrix imat;
	if (matrix_inverse(&mat, &imat)) {
		// invalid matrix
		*touch = NULL;
		return 0;
	}
	int *m = imat.m;

	int xx = (x * m[0] + y * m[2]) / 1024 + m[4];
	int yy = (x * m[1] + y * m[3]) / 1024 + m[5];

	int testin;
	struct sprite * tmp = s;
	switch (s->type) {
	case TYPE_PICTURE:
		testin = test_quad(s->s.pic, xx, yy);
		break;
	case TYPE_POLYGON:
		testin = test_polygon(s->s.poly, xx, yy);
		break;
	case TYPE_LABEL:
		testin = test_label(s->s.label, xx, yy);
		break;
	case TYPE_PANNEL:
		testin = test_pannel(s->s.pannel, xx, yy);
		break;
	case TYPE_ANCHOR:
		*touch = NULL;
		return 0;
	default:
		// todo : invalid type
		*touch = NULL;
		return 0;
	}

	if (testin) {
		*touch = tmp;
		return s->message;
	} else {
		*touch = NULL;
		return 0;
	}
}
Пример #14
0
 /**
  * Inverse matrix itself
  * The function returns 1 on success, 0 on failure.
  */
 int inverse(void) {
   matrixDbgCheck(c == r, "matrix must be square");
   return matrix_inverse(c, M);
 }
Пример #15
0
int linearprogram(double a[],double b[],double c[],int m,int n,double x[])
{
  int i,k,j,*js;
  double s,z,dd,y,*p,*d;
  RM ma={m,m+n,a},mp,md;

  js=malloc(m*sizeof(int));
  p=malloc(m*m*sizeof(double));
  d=malloc(m*(m+n)*sizeof(double));
  mp.row=mp.col=m;mp.data=p;
  md.data=d;
  for(i=0;i<m;i++)
  {
    js[i]=n+i;
  }
  s=0.0;
  while(1)
  {
    for(i=0;i<m;i++)
      for(j=0;j<m;j++)
        p[i*m+j]=a[i*(m+n)+js[j]];
    if(matrix_inverse(&mp)!=0)
    {
      x[n]=s;
      free(js);free(p);free(d);
      return 0;
    }
    matrixmul(&mp,&ma,&md);
    for(i=0;i<m+n;i++)
    {
      x[i]=0.0;
    }
    for(i=0;i<m;i++)
    {
      for(j=0,s=0.0;j<m;j++)
        s+=p[i*m+j]*b[j];
      x[js[i]]=s;
    }
    k=-1;dd=1.0e-35;
    for(j=0;j<m+n;j++)
    {
      for(i=0,z=0.0;i<m;i++)
        z+=c[js[i]]*d[i*(m+n)+j];
      z-=c[j];
      if(z>dd){dd=z;k=j;}
    }
    if(k==-1)
    {
      for(j=0,s=0.0;j<n;j++)
        s+=c[j]*x[j];
      x[n]=s;
      free(js);free(p);free(d);
      return 1;
    }
    j=-1;
    dd=1.0e+20;
    for(i=0;i<m;i++)
      if(d[i*(m+n)+k]>=1.0e-20)
      {
        y=x[js[i]]/d[i*(m+n)+k];
        if(y<dd){dd=y;j=i;}
      }
      if(j==-1)
      {
        x[n]=s;
        free(js);free(p);free(d);
        return 0;
      }
      js[j]=k;
  }
}
void LucasKanadeInverseAffine(double *I, int *sizeI, double *Wn_padded, double *p_in, double *I_template_padded, int *sizeT, struct options* Options, double *p_out, double *I_roi, double *T_error) {
    double *G_x1, *G_y1, *G_x2, *G_y2, *I_template, *Wn;
    /* Loop variables */
    int i, j, k;
    /* Coordinates */
    double x, y;
    /* Temp variable */
    int b;
    /* 1D Index variables */
    int Index1, Index2, Index3;
    /* Tsize without padding */
    int sizeTc[2];
    /* Template Center */
    double TemplateCenter[2];
    /* Gamma (is like the jacobian in Lucas Kanade Affine) */
    double Gamma_x[6], Gamma_y[6];
    /* Inverse parameter Gamma */
    double *Gamma_p_inv;
    /* Steepest decent images */
    double *gG1, *gG2, *gG1t, *gG2t;
    /* Hessian */
    double *H_mod1, *H_mod2, *H_mod1t, *H_mod2t;
    double *H_mod1_inv, *H_mod2_inv, *H_mod1t_inv, *H_mod2t_inv;
    /* Number of template pixels */
    int nTPixels;
    /* Warp/ pixel transformation matrix */
    double W_xp[9];
    /* Warped image */
    double *I_warped;
    /* Error between warped image and template */
    double *I_error;
    /* . */
    double sum_xy[6];
    /* Modified and unmodified Update of affine parameters */
    double delta_p_mod[6], delta_p[6];
    /* Norm storage */
    double norm_s;
    
    
    G_x1 = (double*)malloc( sizeT[0]*sizeT[1]*sizeof(double) );
    G_y1 = (double*)malloc( sizeT[0]*sizeT[1]*sizeof(double) );
    G_x2 = (double*)malloc( sizeT[0]*sizeT[1]*sizeof(double) );
    G_y2 = (double*)malloc( sizeT[0]*sizeT[1]*sizeof(double) );
    Wn = (double*)malloc( sizeT[0]*sizeT[1]*sizeof(double) );
    
    
    /* 3: Evaluate the Gradient of the template */
    matrix_derivatives(I_template_padded, Options->RoughSigma, G_x1, G_y1, sizeT);
    matrix_derivatives(I_template_padded, Options->FineSigma, G_x2, G_y2, sizeT);
    
    /* Remove the padding from the derivatives */
    b=Options->Padding;
    sizeTc[0]=sizeT[0]-b*2; sizeTc[1]=sizeT[1]-b*2;
    I_template = (double*)malloc( sizeT[0]*sizeT[1]*sizeof(double) );
    if((sizeTc[0]<0)||(sizeTc[1]<0)){ mexErrMsgTxt("2xPadding must be smaller than template size"); }
    for(j=0;j<sizeTc[1];j++) {
        for(i=0;i<sizeTc[0];i++) {
            Index1=i+j*sizeTc[0]; Index2=(i+b)+(j+b)*sizeT[0];
            G_x1[Index1]=G_x1[Index2];
            G_y1[Index1]=G_y1[Index2];
            G_x2[Index1]=G_x2[Index2];
            G_y2[Index1]=G_y2[Index2];
            Wn[Index1]=Wn_padded[Index2];
            I_template[Index1]=I_template_padded[Index2];
        }
    }
    
    TemplateCenter[0]=((double)sizeTc[0])/2.0;
    TemplateCenter[1]=((double)sizeTc[1])/2.0;
    nTPixels=sizeTc[0]*sizeTc[1];
    
    /* memory for steepest decent images */
    gG1  = (double*)malloc( 6*nTPixels*sizeof(double) );
    gG2  = (double*)malloc( 6*nTPixels*sizeof(double) );
    gG1t  = (double*)malloc( 2*nTPixels*sizeof(double) );
    gG2t  = (double*)malloc( 2*nTPixels*sizeof(double) );
    
    /* memory for hessian and inverted hessian */
    H_mod1 = (double*)malloc( 6*6*sizeof(double) );
    H_mod2 = (double*)malloc( 6*6*sizeof(double) );
    H_mod1t = (double*)malloc( 2*2*sizeof(double) );
    H_mod2t = (double*)malloc( 2*2*sizeof(double) );
    H_mod1_inv = (double*)malloc( 6*6*sizeof(double) );
    H_mod2_inv = (double*)malloc( 6*6*sizeof(double) );
    H_mod1t_inv = (double*)malloc( 2*2*sizeof(double) );
    H_mod2t_inv = (double*)malloc( 2*2*sizeof(double) );
    for (i=0; i<36; i++) {  H_mod1[i]=0; H_mod2[i]=0; H_mod1_inv[i]=0; H_mod2_inv[i]=0;  }
    for (i=0; i<4; i++) {  H_mod1t[i]=0; H_mod2t[i]=0; H_mod1t_inv[i]=0; H_mod2t_inv[i]=0;  }
    
    /* memory for warped image and error image */
    I_warped = (double*)malloc( nTPixels*sizeof(double) );
    I_error = (double*)malloc( nTPixels*sizeof(double) );
    
    /* memory for inverse parameter gamma, and initialize to zero */
    Gamma_p_inv = (double*)malloc(36*sizeof(double) );
    for (i=0; i<36; i++) {  Gamma_p_inv[i]=0; }
    
    for(j=0;j<sizeTc[1];j++) {
        for(i=0;i<sizeTc[0];i++) {
            Index1=i+j*sizeTc[0];
            x=((double)i)-TemplateCenter[0]; y=((double)j)-TemplateCenter[1];
            
            /* 4: Evaluate Gamma(x) (Same as Jacobian in normal LK_affine) */
            Gamma_x[0]=x; Gamma_x[1]=0; Gamma_x[2]=y; Gamma_x[3]=0; Gamma_x[4]=1; Gamma_x[5]=0;
            Gamma_y[0]=0; Gamma_y[1]=x; Gamma_y[2]=0; Gamma_y[3]=y; Gamma_y[4]=0; Gamma_y[5]=1;
            
            /* 5: Compute the modified steepest descent images gradT * Gamma(x) */
            Index2=Index1*6;
            for(k=0; k<6; k++) {
                gG1[Index2+k]=Gamma_x[k]*G_x1[Index1]+Gamma_y[k]*G_y1[Index1];
                gG2[Index2+k]=Gamma_x[k]*G_x2[Index1]+Gamma_y[k]*G_y2[Index1];
            }
            
            /* 5, for translation only */
            Index3=Index1*2;
            gG1t[Index3+0]=G_x1[Index1];
            gG1t[Index3+1]=G_y1[Index1];
            gG2t[Index3+0]=G_x2[Index1];
            gG2t[Index3+1]=G_y2[Index1];
            
            /* 6: Compute the modified Hessian H* using equation 58 */
            matrixAddvectorXvectorWeighted(&gG1[Index2], Wn[Index1], 6, H_mod1);
            matrixAddvectorXvectorWeighted(&gG2[Index2], Wn[Index1], 6, H_mod2);
            matrixAddvectorXvectorWeighted(&gG1t[Index3], Wn[Index1], 2, H_mod1t);
            matrixAddvectorXvectorWeighted(&gG2t[Index3], Wn[Index1], 2, H_mod2t);
            
            /* Compute the inverse hessians */
            matrix_inverse(H_mod1, H_mod1_inv, 6);
            matrix_inverse(H_mod2, H_mod2_inv, 6);
            matrix_inverse(H_mod1t, H_mod1t_inv, 2);
            matrix_inverse(H_mod2t, H_mod2t_inv, 2);
        }
    }
    
    
    /* Copy parameters to output/working parameters */
    memcpy(p_out, p_in, 6*sizeof(double));
    
    /* Lucas Kanade Main Loop */
    for (i=0; i<(Options->TranslationIterations+Options->AffineIterations); i++) {
        /* W(x;p) */
        W_xp[0]=1+p_out[0];  W_xp[1]=p_out[2];   W_xp[2]=p_out[4];
        W_xp[3]=p_out[1];    W_xp[4]=1+p_out[3]; W_xp[5]=p_out[5];
        W_xp[6]=0;           W_xp[7]=0;          W_xp[8]=1;
        
        /* 1: Warp I with W(x;p) to compute I(W(x;p)) */
        transformimage_gray(I, sizeI, sizeTc, W_xp, I_warped);
        
        /* 2: Compute the error image I(W(x;p))-T(x) */
        matrix_min_matrix(I_warped, I_template, nTPixels, I_error);
        
        /* Break if outside image */
        if((p_out[4]>(sizeI[0]-1))||(p_out[5]>(sizeI[1]-1))||(p_out[4]<0)||(p_out[5]<0)) { break; }
        
        /* First itterations only do translation updates for more robustness */
        /* and after that Affine. */
        
        if(i>=Options->TranslationIterations) {
            /* Affine parameter optimalization */
            
            /* 7: Computer sum_x [gradT*Gamma(x)]^T (I(W(x;p))-T(x)] */
            for (j=0; j<6; j++) {sum_xy[j]=0;}
            if(i<Options->SigmaIterations) {
                for (j=0; j<nTPixels; j++) {
                    for (k=0; k<6; k++) { sum_xy[k]+=Wn[j]*gG1[k+j*6]*I_error[j]; }
                }
            }
            else {
                for (j=0; j<nTPixels; j++) {
                    for (k=0; k<6; k++) { sum_xy[k]+=Wn[j]*gG2[k+j*6]*I_error[j]; }
                }
            }
            
            /* 8: Computer delta_p using Equation 61 */
            for (j=0; j<6; j++) {delta_p_mod[j]=0;}
            if(i<Options->SigmaIterations) {
                for (j=0; j<6; j++) {
                    for (k=0; k<6; k++) { delta_p_mod[j]+=H_mod1_inv[k*6+j]*sum_xy[k]; }
                }
            }
            else {
                for (j=0; j<6; j++) {
                    for (k=0; k<6; k++) { delta_p_mod[j]+=H_mod2_inv[k*6+j]*sum_xy[k]; }
                }
            }
        }
        else {
            /* Translation parameter optimalization */
            
            /* 7: Computer sum_x [gradT*Gamma(x)]^T (I(W(x;p))-T(x)] */
            sum_xy[0]=0; sum_xy[1]=0;
            if(i<Options->SigmaIterations) {
                for (j=0; j<nTPixels; j++) {
                    sum_xy[0]+=gG1t[0+j*2]*I_error[j];
                    sum_xy[1]+=gG1t[1+j*2]*I_error[j];
                }
            }
            else {
                for (j=0; j<nTPixels; j++) {
                    sum_xy[0]+=gG2t[0+j*2]*I_error[j];
                    sum_xy[1]+=gG2t[1+j*2]*I_error[j];
                }
            }
            
            /* 8: Computer delta_p using Equation 61 */
            for (j=0; j<6; j++) {delta_p_mod[j]=0;}
            if(i<Options->SigmaIterations) {
                for (j=0; j<2; j++) {
                    for (k=0; k<2; k++) { delta_p_mod[j+4]+=H_mod1t_inv[k*2+j]*sum_xy[k]; }
                }
            }
            else {
                for (j=0; j<2; j++) {
                    for (k=0; k<2; k++) { delta_p_mod[j+4]+=H_mod2t_inv[k*2+j]*sum_xy[k]; }
                }
            }
        }
        
        /* 9: Compute Gamma(p)^-1 and Update the parameters p <- p + delta_p */
        Gamma_p_inv[0]=1+p_out[0];   Gamma_p_inv[6]=p_out[2];      Gamma_p_inv[1]=p_out[1];
        Gamma_p_inv[7]=1+p_out[3];   Gamma_p_inv[14]=1+p_out[0];   Gamma_p_inv[20]=p_out[2];
        Gamma_p_inv[15]=p_out[1];    Gamma_p_inv[21]=1+p_out[3];   Gamma_p_inv[28]=1+p_out[0];
        Gamma_p_inv[34]=p_out[2];    Gamma_p_inv[29]=p_out[1];     Gamma_p_inv[35]=1+p_out[3];

  
        for (j=0; j<6; j++) {delta_p[j]=0;}
        for (j=0; j<6; j++) {
            for (k=0; k<6; k++) { delta_p[j]+=Gamma_p_inv[k*6+j]*delta_p_mod[k]; }
        }

        for (j=0; j<6; j++) { p_out[j]-=delta_p[j];}
        
        /* Break if position is already good enough */
        norm_s=0; for (j=0; j<6; j++) {norm_s+=pow2(delta_p[j]);}
        if((norm_s<pow2(Options->TolP))&&(i>=Options->TranslationIterations)) { break; }
    }
   
    /* Warp to give a roi back with padding */
    transformimage_gray(I, sizeI, sizeT, W_xp, I_roi);
    
    norm_s=0; for(j=0; j<nTPixels; j++) {norm_s+=pow2(I_error[j]);}
    T_error[0]=norm_s/((double)nTPixels);

    /* Free memory */
    free(G_x1); free(G_y1); free(G_x2); free(G_y2);
    free(gG1); free(gG2); free(gG1t); free(gG2t);
    free(H_mod1); free(H_mod2); free(H_mod1t); free(H_mod2t);
    free(H_mod1_inv); free(H_mod2_inv); free(H_mod1t_inv); free(H_mod2t_inv);
    free(I_warped); free(I_error); free(Gamma_p_inv);
}
Пример #17
0
int
main (int argc, char **argv)
{
    SDL_Surface *image;
    nile_t *nl;
    char mem[500000];
    uint32_t texture_pixels[TEXTURE_WIDTH * TEXTURE_HEIGHT] = {0};
    real angle = 0;
    real scale;

    if (SDL_Init (SDL_INIT_VIDEO) == -1)
        DIE ("SDL_Init failed: %s", SDL_GetError ());
    if (!SDL_SetVideoMode (DEFAULT_WIDTH, DEFAULT_HEIGHT, 0,
                           SDL_HWSURFACE | SDL_ANYFORMAT | SDL_DOUBLEBUF))
        DIE ("SDL_SetVideoMode failed: %s", SDL_GetError ());
    image = SDL_GetVideoSurface ();

    nl = nile_new (NTHREADS, mem, sizeof (mem));

    ilInit ();
    ilBindImage (iluGenImage ());
    if (argc < 3)
        return -1;
    printf ("loading: %s\n", argv[1]);
    ilLoadImage (argv[1]);
    ilCopyPixels (0, 0, 0, TEXTURE_WIDTH, TEXTURE_HEIGHT, 1, IL_BGRA,
                  IL_UNSIGNED_BYTE, &texture_pixels);
    int filter = atoi (argv[2]);

    for (;;) {
        angle += 0.001;
        scale = fabs (angle - (int) angle - 0.5) * 10;
        SDL_Event event;
        if (SDL_PollEvent (&event) && event.type == SDL_QUIT)
            break;

        SDL_FillRect (image, NULL, 0);
        SDL_LockSurface (image);

            matrix_t M = matrix_new ();
            M = matrix_translate (M, 250, 250);
            M = matrix_rotate (M, angle);
            M = matrix_scale (M, scale, scale);
            M = matrix_translate (M, -250, -250);
            matrix_t I = matrix_inverse (M);

            nile_Kernel_t *texture =
                gezira_ReadFromImage_ARGB32 (nl, texture_pixels, TEXTURE_WIDTH,
                                             TEXTURE_HEIGHT, TEXTURE_WIDTH);
            /*
             */
            texture = nile_Pipeline (nl,
                    gezira_ImageExtendReflect (nl, TEXTURE_WIDTH, TEXTURE_HEIGHT),
                    texture, NULL);
            if (filter == 2)
                texture = gezira_BilinearFilter (nl, texture);
            if (filter == 3)
                texture = gezira_BicubicFilter (nl, texture);
            /*
             */
            texture = nile_Pipeline (nl, 
                gezira_TransformPoints (nl, I.a, I.b, I.c, I.d, I.e - 150, I.f - 125),
                texture, NULL);
            nile_Kernel_t *pipeline = nile_Pipeline (nl,
                gezira_TransformBeziers (nl, M.a, M.b, M.c, M.d, M.e, M.f),
                gezira_ClipBeziers (nl, 0, 0, DEFAULT_WIDTH, DEFAULT_HEIGHT),
                gezira_Rasterize (nl),
                gezira_ApplyTexture (nl, texture),
                gezira_WriteToImage_ARGB32 (nl, image->pixels,
                                            DEFAULT_WIDTH, DEFAULT_HEIGHT,
                                            image->pitch / 4),
                NULL);

            nile_feed (nl, pipeline, path, 6, path_n, 1);
            nile_sync (nl);

        SDL_UnlockSurface (image);
        SDL_Flip (image);
    }

    nile_free (nl);
    printf ("done\n");

    return 0;
}
Пример #18
0
/* This function is the key to GPS solution, so it's commented
 * liberally.  It does a single step of a multi-dimensional
 * Newton-Raphson solution for the variables X, Y, Z (in ECEF) plus
 * the clock offset for each receiver used to make pseudorange
 * measurements.  The steps involved are roughly the following:
 *
 *     1. Account for the Earth's rotation during transmission
 *
 *     2. Estimate the ECEF position for each satellite measured using
 *     the downloaded ephemeris
 *
 *     3. Compute the Jacobian of pseudorange versus estimated state.
 *     There's no explicit differentiation; it's done symbolically
 *     first and just coded as a "line of sight" vector.
 *
 *     4. Get the inverse of the Jacobian times its transpose.  This
 *     matrix is normalized to one, but it tells us the direction we
 *     must move the state estimate during this step.
 *
 *     5. Multiply this inverse matrix (H) by the transpose of the
 *     Jacobian (to yield X).  This maps the direction of our state
 *     error into a direction of pseudorange error.
 *
 *     6. Multiply this matrix (X) by the error between the estimated
 *     (ephemeris) position and the measured pseudoranges.  This
 *     yields a vector of corrections to our state estimate.  We apply
 *     these to our current estimate and recurse to the next step.
 *
 *     7. If our corrections are very small, we've arrived at a good
 *     enough solution.  Solve for the receiver's velocity (with
 *     vel_solve) and do some bookkeeping to pass the solution back
 *     out.
 */
static double pvt_solve(double rx_state[],
                        const u8 n_used,
                        const navigation_measurement_t const nav_meas[n_used],
                        double H[4][4])
{
  double p_pred[n_used];

  /* Vector of prediction errors */
  double omp[n_used];

  /* G is a geometry matrix tells us how our pseudoranges relate to
   * our state estimates -- it's the Jacobian of d(p_i)/d(x_j) where
   * x_j are x, y, z, Δt. */
  double G[n_used][4];
  double Gtrans[4][n_used];
  double GtG[4][4];

  /* H is the square of the Jacobian matrix; it tells us the shape of
     our error (or, if you prefer, the direction in which we need to
     move to get a better solution) in terms of the receiver state. */

  /* X is just H * Gtrans -- it maps our pseudoranges onto our
   * Jacobian update */
  double X[4][n_used];

  double tempv[3];
  double los[3];
  double xk_new[3];
  double tempd;
  double correction[4];

  for (u8 j=0; j<4; j++) {
    correction[j] = 0.0;
  }

  for (u8 j = 0; j < n_used; j++) {
    /* The satellite positions need to be corrected for Earth's rotation during
     * the signal time of flight. */
    /* TODO: Explain more about how this corrects for the Sagnac effect. */

    /* Magnitude of range vector converted into an approximate time in secs. */
    vector_subtract(3, rx_state, nav_meas[j].sat_pos, tempv);
    double tau = vector_norm(3, tempv) / GPS_C;

    /* Rotation of Earth during time of flight in radians. */
    double wEtau = GPS_OMEGAE_DOT * tau;

    /* Apply linearlised rotation about Z-axis which will adjust for the
     * satellite's position at time t-tau. Note the rotation is through
     * -wEtau because it is the ECEF frame that is rotating with the Earth and
     * hence in the ECEF frame free falling bodies appear to rotate in the
     * opposite direction.
     *
     * Making a small angle approximation here leads to less than 1mm error in
     * the satellite position. */
    xk_new[0] = nav_meas[j].sat_pos[0] + wEtau * nav_meas[j].sat_pos[1];
    xk_new[1] = nav_meas[j].sat_pos[1] - wEtau * nav_meas[j].sat_pos[0];
    xk_new[2] = nav_meas[j].sat_pos[2];

    /* Line of sight vector. */
    vector_subtract(3, xk_new, rx_state, los);

    /* Predicted range from satellite position and estimated Rx position. */
    p_pred[j] = vector_norm(3, los);

    /* omp means "observed minus predicted" range -- this is E, the
     * prediction error vector (or innovation vector in Kalman/LS
     * filtering terms).
     */
    omp[j] = nav_meas[j].pseudorange - p_pred[j];

    /* Construct a geometry matrix.  Each row (satellite) is
     * independently normalized into a unit vector. */
    for (u8 i=0; i<3; i++) {
      G[j][i] = -los[i] / p_pred[j];
    }

    /* Set time covariance to 1. */
    G[j][3] = 1;

  } /* End of channel loop. */

  /* Solve for position corrections using batch least-squares.  When
   * all-at-once least-squares estimation for a nonlinear problem is
   * mixed with numerical iteration (not time-series recursion, but
   * iteration on a single set of measurements), it's basically
   * Newton's method.  There's a reasonably clear explanation of this
   * in Wikipedia's article on GPS.
   */

  /* Gt := G^{T} */
  matrix_transpose(n_used, 4, (double *) G, (double *) Gtrans);
  /* GtG := G^{T} G */
  matrix_multiply(4, n_used, 4, (double *) Gtrans, (double *) G, (double *) GtG);
  /* H \elem \mathbb{R}^{4 \times 4} := GtG^{-1} */
  matrix_inverse(4, (const double *) GtG, (double *) H);
  /* X := H * G^{T} */
  matrix_multiply(4, 4, n_used, (double *) H, (double *) Gtrans, (double *) X);
  /* correction := X * E (= X * omp) */
  matrix_multiply(4, n_used, 1, (double *) X, (double *) omp, (double *) correction);

  /* Increment ecef estimate by the new corrections */
  for (u8 i=0; i<3; i++) {
    rx_state[i] += correction[i];
  }

  /* Set the Δt estimates according to this solution */
  for (u8 i=3; i<4; i++) {
    rx_state[i] = correction[i];
  }

  /* Look at the magnintude of the correction to see if
   * the solution has converged yet.
   */
  tempd = vector_norm(3, correction);
  if(tempd > 0.001) {
    /* The solution has not converged, return a negative value to
     * indicate that we should continue iterating.
     */
    return -tempd;
  }

  /* The solution has converged! */

  /* Perform the velocity solution. */
  vel_solve(&rx_state[4], n_used, nav_meas, (const double (*)[4]) G, (const double (*)[n_used]) X);

  return tempd;
}
Пример #19
0
int SQFitting::estimateParameters(const pcl::PointCloud<pcl::PointXYZ>& cloud, SQParameters& sqParams, const int& eigenVector) {
	double t[4][4] = {0};

	// Check if cloud contains enough data points
	int len = (int) cloud.points.size();
	if (len < 13) { // changed from 13
		return -1;
	}

	// Find center of gravity
	double a, b, c, x, y, z, xx, yy, zz, cxy, cxz, cyz, count = 0.0;
	
	x = y = z = 0; 
	xx = yy = zz = cxy = cyz = cxz = 0; 
	count = 0;

	for(int i = 0; i < len; i++) {
		pcl::PointXYZ pnt = cloud.points[i];
		a = (double) pnt.x;
		b = (double) pnt.y;
		c = (double) pnt.z;

		x += a;
		y += b;
		z += c;

		xx += a*a;
		yy += b*b;
		zz += c*c;

		cxz += a*c;
		cyz += b*c;
		cxy += a*b;

		count++;
	}

	// Compute center of gravity, covariance and variance
	double avgx, avgy, avgz;
	avgx = x/count;
	avgy = y/count;
	avgz = z/count;

	xx  /= count;
	yy  /= count;
	zz  /= count;

	cxz /= count;
	cyz /= count;
	cxy /= count;

    xx  -= (avgx * avgx);
    yy  -= (avgy * avgy);
    zz  -= (avgz * avgz);

    cxy -= (avgx * avgy);
    cxz -= (avgx * avgz);
    cyz -= (avgy * avgz);

	// Build covariance matrix
	double aa[3][3];
	aa[0][0] =  xx;   aa[0][1] = cxy; aa[0][2] = cxz;
	aa[1][0] = cxy;   aa[1][1] =  yy; aa[1][2] = cyz;
	aa[2][0] = cxz;   aa[2][1] = cyz; aa[2][2] =  zz;

	// Compute eigenvectors and eigenvalues
	double	d[3], m_brain[3][3], m_inverse[3][3];
	double lambdas[3];
	double vectors[3][3];
	int    nrot;

	/*
	 * Using the covariance matrix, we get its eigenvalues
	 * and eigenvectors
	 */
	cvJacobiEigens_64d((double*) aa, (double*) m_brain, (double*) d, 3, 0.0);

	// Find the vector with the largest eigenvalue
	double max_eigen = -1000000000.0;
	int vectorIndex;
	
	if(d[0] > max_eigen) {
		max_eigen = d[0];
		vectorIndex = 0;
	}
	if(d[1] > max_eigen) {
		max_eigen = d[1]; 
		vectorIndex = 1;
	}
	if(d[2] > max_eigen) {
		max_eigen = d[2]; 
		vectorIndex = 2;
	}
	
	for (int j = 0; j < 3; j++) {
		for (int k = 0; k < 3; k++) {
			m_inverse[j][k] = m_brain[j][k];
		}
	}

	// Aligning max eigenvector with z axis
#if 0

	for (int j = 0; j < 3; j++) { 
		m_inverse[j][2] = m_brain[j][eigenVector];
		m_inverse[j][eigenVector] = m_brain[j][2];
	}

	
	if (vectorIndex != 2) {
		for (int j = 0; j < 3; j++) {
			m_inverse[j][vectorIndex] = -m_inverse[j][vectorIndex];
		}
	}	

#endif

	for (int j = 0; j < 3; j++) {
		for (int k = 0; k < 3; k++) {
			m_brain[j][k] = m_inverse[j][k];
		}
	}
	
	// Build transformation matrix (rotation & translation)
	for (int i = 0; i < 3; i++) {
		for (int j = 0; j < 3; j++){
			t[i][j] = m_brain[i][j];
	    }
	}
	
	t[0][3] = avgx;
	t[1][3] = avgy;
	t[2][3] = avgz;
	t[3][3] =  1;

	t[3][0] = t[3][1] = t[3][2] = 0.0;

	// Set / Calculate initial parameter
	sqParams.e1 = 1;
	sqParams.e2 = 1;
	sqParams.kx = 0;
	sqParams.ky = 0;
	
	double xmin, ymin, zmin, xmax, ymax, zmax;
	xmin = ymin = zmin = 100000.00;
	xmax = ymax = zmax = -100000.00;

	double tInv[4][4];
	matrix_inverse(t, tInv);

	double vector[4];
	for(int i = 0; i < len; i++) {
		pcl::PointXYZ pnt = cloud.points[i];
		
		vector[0] = (double) pnt.x;
		vector[1] = (double) pnt.y;
		vector[2] = (double) pnt.z;
		vector[3] = 1;

		double result[4];
		matrix_mult(vector, tInv, result);
		a = result[0]; 
		b = result[1]; 
		c = result[2];

		if(xmin > a)  xmin = a;     
		if(xmax < a)  xmax = a;

		if(ymin > b)  ymin = b;     
		if(ymax < b)  ymax = b;

		if(zmin > c)  zmin = c;     
		if(zmax < c)  zmax = c;
	}

	double r1, r2, r3;

	r1 = atan2(t[1][2], t[0][2]);
	r1 = r1 + M_PI;
	r2 = atan2(cos(r1) * t[0][2] + sin(r1) * t[1][2], t[2][2]);	
	r3 = atan2(-sin(r1) * t[0][0] + cos(r1) * t[1][0], -sin(r1) * t[0][1] + cos(r1) * t[1][1]);
	
	sqParams.phi = r1;
	sqParams.theta = r2;
	sqParams.psi = r3;

	sqParams.px = t[0][3] + (xmax + xmin) / 2;   
	sqParams.py = t[1][3] + (ymax + ymin) / 2;
	sqParams.pz = t[2][3] + (zmax + zmin) / 2;
 
	double ta1, ta2, ta3;

	ta1 = (xmax - xmin)/2;
	ta2 = (ymax - ymin)/2;
	ta3 = (zmax - zmin)/2;

	sqParams.a1 = ta1;
	sqParams.a2 = ta2;
	sqParams.a3 = ta3;	

	return 0;
}
Пример #20
0
//only works for 2x2 matrices
matrix operator/(matrix A, matrix B){
	return matrix_multiply(A, matrix_inverse(B));
}