double GLGPU3DDataset::Flux(int face) const
{
// TODO: pre-compute the flux
switch (face) {
case 0: return -dx() * dy() * Bz();
case 1: return -dy() * dz() * Bx();
case 2: return -dz() * dx() * By();
case 3: return dx() * dy() * Bz();
case 4: return dy() * dz() * Bx();
case 5: return dz() * dx() * By();
default: assert(false);
}
return 0.0;
}
float GLGPUDataset::QP(const float X0[], const float X1[]) const
{
const float *L = Lengths(),
*O = Origins();
float d[3] = {X1[0] - X0[0], X1[1] - X0[1], X1[2] - X0[2]};
int p[3] = {0}; // 0: not crossed; 1: positive; -1: negative
for (int i=0; i<3; i++) {
d[i] = X1[i] - X0[i];
if (d[i]>L[i]/2) {d[i] -= L[i]; p[i] = 1;}
else if (d[i]<-L[i]/2) {d[i] += L[i]; p[i] = -1;}
}
const float X[3] = {X0[0] - O[0], X0[1] - O[1], X0[2] - O[2]};
if (By()>0 && p[0]!=0) { // By>0
return p[0] * L[0] * (Bz()*X[1] - By()*X[2]);
} else if (p[1]!=0) {
return p[1] * L[1] * (Bx()*X[2] - Bz()*X[0]);
} else return 0.0;
}
double GLGPU3DDataset::GaugeTransformation(const double X0[], const double X1[]) const
{
double gx, gy, gz;
double dx = X1[0] - X0[0],
dy = X1[1] - X0[1],
dz = X1[2] - X0[2];
double x = X0[0] + 0.5*dx,
y = X0[1] + 0.5*dy,
z = X0[2] + 0.5*dz;
if (By()>0) { // Y-Z gauge
gx = dx * Kex();
gy =-dy * x * Bz(); // -dy*x^hat*Bz
gz = dz * x * By(); // dz*x^hat*By
} else { // X-Z gauge
gx = dx * y * Bz() + dx * Kex(); // dx*y^hat*Bz + dx*K
gy = 0;
gz =-dz * y * Bx(); // -dz*y^hat*Bx
}
return gx + gy + gz;
}
int sc_main( int ac, char* av[] )
{
sc_uint_base Bx(WIDTH), By(WIDTH);
for(unsigned int i = 0; i < WIDTH; i++){
cout << "i = " << i << endl;
for(unsigned int j=0; j < COUNT; j++ ){
/* ( By.range(WIDTH-1, i+1), By.range(i,0) ) = Bx;
sc_assert( By == Bx );
*/
Bx = j;
sc_assert( (bool) Bx[i] == (j & (1<<i)? 1 : 0) );
By[i] = Bx[i];
sc_assert( (bool) By[i] == (j & (1<<i)? 1 : 0) );
}
}
return 0;
}
int sc_main( int ac, char* av[] )
{
sc_int_base Bx(WIDTH),By(WIDTH);
for(unsigned int i = 0; i < WIDTH; i++){
cout << "i = " << i << endl;
for(unsigned int j=0; j < COUNT; j++ ){
/* ( By.range(WIDTH-1, i+1), By.range(i,0) ) = Bx;
assert( By == Bx );
*/
Bx = j;
By.range(WIDTH-1,i) = Bx.range(WIDTH-1,i);
if(i >= 1)
By.range(i-1,0) = Bx.range(i-1,0);
assert( Bx == By );
}
}
return 0;
}
void STWarp<T>::multiscaleIteration(WarpingField<T> &warpField) {
for (int warpIter = 0; warpIter < params.warpIterations; ++warpIter) {
if(params.verbosity > 1) {
printf(" - warp iteration %02d\n",warpIter+1);
}
// Get derivatives
if(params.verbosity > 1) {
printf(" - computing derivatives...");
}
Video<T> Bx(videoB->size());
Video<T> By(videoB->size());
Video<T> Bt(videoB->size());
Video<T> C(videoB->size());
computePartialDerivatives(warpField,Bx,By,Bt,C);
if(params.verbosity >1) {
printf("done.\n");
}
// marginal warpField update
WarpingField<T> dWarpField = WarpingField<T>(warpField.size());
warpingIteration(warpField, Bx, By, Bt, C, dWarpField);
if (params.limitUpdate) {
if(params.verbosity > 1) {
printf(" - limiting warp update to [-1,1]");
}
dWarpField.clamp(-1,1);
}
// w <- w + dw
warpField.add(dWarpField);
denoiseWarpingField(warpField);
} // end of warp iteration
}
void Filter::Kalman(Joint joint, double &dx, double &dy)
{
Kalmans[Kalman_count++] = joint;
Kalman_count = Kalman_count % Kalman_limit;
Kalman_num++;
if (Kalman_num > Kalman_limit)
{
Kalman_num = Kalman_limit;
}
if (Kalman_num < Kalman_limit)
{
dx = joint.Position.X;
dy = joint.Position.Y;
return;
}
else
{
//X, Y
int haha;
haha = 1;
double x[5], y[5];
int pos = Kalman_count;
for (int i = 0; i < 5; i++)
{
x[i] = Kalmans[pos].Position.X;
y[i] = Kalmans[pos].Position.Y;
pos++;
pos = pos%Kalman_limit;
}
//求系数Ax, Ay
double Ax[5] = {
/*a0*/ x[0],
/*a1*/ 4 * (x[1] - x[0]) - 3 * x[2] + 4 * x[3] / 3 - x[4] / 4,
/*a2*/ 11 * x[4] / 24 - 7 * x[3] / 3 + 19 * x[2] / 4 - 13 * (x[1] - x[0]) / 3,
/*a3*/ x[4] / 3 - 7 * x[3] / 6 + x[2] - (x[1] - x[0]) / 2,
/*a4*/ (x[4] - 4 * x[3] + 6 * x[2] + 4 * (x[1] - x[0])) / 24
};
double Ay[5] = {
/*a0*/ y[0],
/*a1*/ 4 * (y[1] - y[0]) - 3 * y[2] + 4 * y[3] / 3 - y[4] / 4,
/*a2*/ 11 * y[4] / 24 - 7 * y[3] / 3 + 19 * y[2] / 4 - 13 * (y[1] - y[0]) / 3,
/*a3*/ y[4] / 3 - 7 * y[3] / 6 + y[2] - (y[1] - y[0]) / 2,
/*a4*/ (y[4] - 4 * y[3] + 6 * y[2] + 4 * (y[1] - y[0])) / 24
};
//求转换矩阵Fx, Fy
Matrix Fx(4, 4,
new double[16]{
1, 1, -0.5, (Ax[1] + 6 * Ax[3] - 4 * Ax[4]) / (24 * Ax[4]),
0, 1, 1, 0.5,
0, 0, 1, 1,
0, 0, 0, 1});
Matrix Fy(4, 4,
new double[16]{
1, 1, -0.5, (Ay[1] + 6 * Ay[3] - 4 * Ay[4]) / (24 * Ay[4]),
0, 1, 1, 0.5,
0, 0, 1, 1,
0, 0, 0, 1});
//求ε(t|t-1)
Matrix ex(4, 4), ey(4, 4);
ex = Fx*Kalman_ex*(!Fx);
ey = Fy*Kalman_ey*(!Fy);
//cout << "ex" << endl; ex.print();
//cout << "ey" << endl; ey.print();
Matrix Bx(4, 1), By(4, 1);
//cout << "!Kalman_C" << endl; (!Kalman_C).print();
//cout << "Kalman_vx" << endl; Kalman_vx.print();
//cout << "Kalman_C" << endl; Kalman_C.print();
//cout << "!Kalman_C" << endl; (!Kalman_C).print();
//cout << "Kalman_C*ex" << endl; (Kalman_C*ex).print();
//cout << "Kalman_C*ex*(!Kalman_C)" << endl; (Kalman_C*ex*(!Kalman_C)).print();
//cout << "(~(Kalman_vx + Kalman_C*ex*(!Kalman_C)))" << endl;
//(~(Kalman_vx + Kalman_C*ex*(!Kalman_C))).print();
Bx = ex*(!Kalman_C)*(~(Kalman_vx + Kalman_C*ex*(!Kalman_C)));
//cout << "Bx" << endl; Bx.print();
By = ey*(!Kalman_C)*(~(Kalman_vy + Kalman_C*ey*(!Kalman_C)));
Matrix I4(4, 4);
I4.SetIdentity();
Kalman_Sx = (I4 - Bx*Kalman_C)*(Fx*Kalman_Sx + Kalman_Gx) +
Bx*Matrix(1, 1, new double[1] {joint.Position.X});
//cout << "Kalman_Sx" << endl; Kalman_Sx.print();
Kalman_Sy = (I4 - By*Kalman_C)*(Fy*Kalman_Sy + Kalman_Gy) +
By*Matrix(1, 1, new double[1] {joint.Position.Y});
Kalman_ex = ex - Bx*Kalman_C*ex;
Kalman_ey = ey - By*Kalman_C*ey;
dx = Kalman_Sx.at(0, 0);
dy = Kalman_Sy.at(0, 0);
}
}
float Az(const float X[3], int slot=0) const {if (By()>0) return -X[0]*By(); else return X[1]*Bx();}
// float Ax(const float X[3], int slot=0) const {if (By()>0) return 0; else return -X[1]*Bz();}
float Ay(const float X[3], int slot=0) const {if (By()>0) return X[0]*Bz(); else return 0;}
float Ax(const float X[3], int slot=0) const {if (By()>0) return -Kex(slot); else return -X[1]*Bz()-Kex(slot);}
