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.");
}
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.");
}
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();*/
}
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;
}
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;
}
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;
}
// 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);
}
}
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.");
}
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;
}
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);
}
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);
}
}
// 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! */
}
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;
}
}
/**
* 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);
}
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);
}
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;
}
/* 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;
}
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;
}
//only works for 2x2 matrices
matrix operator/(matrix A, matrix B){
return matrix_multiply(A, matrix_inverse(B));
}
