int problem3_main(){
std::ofstream fout1("problem3.30.txt");
solve(30.0 / 180.0 * acos(-1), fout1);
fout1.close();
std::ofstream fout2("problem3.40.txt");
solve(40.0 / 180.0 * acos(-1), fout2);
fout2.close();
std::ofstream fout3("problem3.50.txt");
solve(50.0 / 180.0 * acos(-1), fout3);
fout3.close();
B2_M = 0.0;
std::ofstream fout4("problem3-no-air.30.txt");
solve(30.0 / 180.0 * acos(-1), fout4);
fout4.close();
std::ofstream fout5("problem3-no-air.40.txt");
solve(40.0 / 180.0 * acos(-1), fout5);
fout5.close();
std::ofstream fout6("problem3-no-air.50.txt");
solve(50.0 / 180.0 * acos(-1), fout6);
fout6.close();
return 0;
}
int main ()
{
string inVec1, inVec2;
ShiftRegister UUT1;
ShiftRegHuff UUT2;
ifstream finp1 ("indata1.tst");
ofstream fout1 ("outdata1.tst");
ifstream finp2 ("indata2.tst");
ofstream fout2 ("outdata2.tst");
char stop_in1 ('0');
char stop_in2 ('0');
finp1 >> inVec1; // Read in first line of testbench data
while (stop_in1 != '.')
{
UUT1.load_input(inVec1); // Load inputs into ShiftRegister
UUT1.update_reg(); // Compute operation based on mode
UUT1.set_output(); // Set DFFs based on computed values
fout1 << UUT1.output_reg(); // Output data to external file
// Gather next data input:
finp1 >> inVec1;
stop_in1 = inVec1[0];
}
fout1 << "END FILESTREAM";
/// Now for the Huffman Model (with the worst case delays):
finp2 >> inVec2; // Read in first line of testbench data
CombLogic CL; // Define combinational logic class
while (stop_in2 != '.')
{
UUT2.load_input(inVec2); // Load inputs into ShiftRegister
CL.update_reg(UUT2); // Apply combinational logic
UUT2.set_output(); // Set DFFs based on computed values
fout2 << UUT2.output_reg(); // Output data to external file
// Gather next data input:
finp2 >> inVec2;
stop_in2 = inVec2[0];
}
return 0;
}
// protected
void
FileOutputStreamTestCase::ctors (void)
{
File f1 (TEST_FILE_NAME);
if (f1.exists () == false)
f1.create ();
// open mode
FileOutputStream fout (TEST_FILE_NAME, OPEN_MODE);
fout.close ();
// open or create mode
File file (TEST_FILE_NAME);
FileOutputStream fout1 (file, OPEN_OR_CREATE_MODE);
fout1.close ();
// read_share
FileOutputStream fout2(TEST_FILE_NAME, CREATE_MODE, READ_SHARE);
fout2.close ();
}
//電磁力スムージング関数
void smoothingF3D(mpsconfig *CON,vector<mpsparticle> &PART,int fluid_number,double *F[3],int t)
{
double le=CON->get_distancebp();
double ep=8.854e-12;///真空の誘電率。
double *newF[3];
for(int D=0;D<3;D++) newF[D]=new double [fluid_number];
if(CON->get_FEM_smn()>0)
{
for(int n=0;n<CON->get_FEM_smn();n++)
{
for(int i=0;i<fluid_number;i++)
{
for(int D=0;D<3;D++) newF[D][i]=F[D][i];
int num=1; //自分自身をｶｳﾝﾄするから1
for(int k=0;k<PART[i].N;k++)
{
int j=PART[i].NEI[k];
if(PART[j].type==FLUID)
{
num++;
for(int D=0;D<3;D++) newF[D][i]+=F[D][j];
}
}
for(int D=0;D<3;D++) newF[D][i]/=num;
}
for(int i=0;i<fluid_number;i++) for(int D=0;D<3;D++) F[D][i]=newF[D][i];
}
}
else if(CON->get_FEM_smn()<0)//表面のみでスムージング
{
int N=-1*CON->get_FEM_smn();
for(int n=0;n<N;n++)
{
for(int i=0;i<fluid_number;i++)
{
for(int D=0;D<3;D++) newF[D][i]=F[D][i];
if(PART[i].surface==ON)
{
int num=1; //自分自身をｶｳﾝﾄするから1
for(int k=0;k<PART[i].N;k++)
{
int j=PART[i].NEI[k];
if(PART[j].surface==ON && PART[j].type==FLUID)
{
num++;
for(int D=0;D<3;D++) newF[D][i]+=F[D][j];
}
}
for(int D=0;D<3;D++) newF[D][i]/=num;
}
}
for(int i=0;i<fluid_number;i++) for(int D=0;D<3;D++) F[D][i]=newF[D][i];
}
}
for(int D=0;D<3;D++) delete [] newF[D];
/////////////////////////////////////*/
////電磁力をプロット
ofstream fp("F.dat");
double xmax=-100; //出力粒子の最大横座標
double ymax=-100; //出力粒子の最大縦座標
double mass=CON->get_particle_mass();
double times=CON->get_times()/mass*le*le*CON->get_FEMtimes();
for(int i=0;i<fluid_number;i++)
{
//if(PART[i].r[A_Y]>-le*0.5&& PART[i].r[A_Y]<+le*0.5)
{
fp<<PART[i].r[A_X]<<"\t"<<PART[i].r[A_Y]<<"\t"<<F[A_X][i]*times<<"\t"<<F[A_Y][i]*times<<endl;
if(PART[i].r[A_X]>xmax) xmax=PART[i].r[A_X];
if(PART[i].r[A_Y]>ymax) ymax=PART[i].r[A_Y];
}
}
xmax+=4*le;//凡例を出す位置を保険をかけて少し斜めに移動
ymax+=4*le;
if(CON->get_legend_F()>0) fp<<xmax<<" "<<ymax<<" "<<CON->get_legend_F()*times<<" "<<0*times<<endl;//最後に凡例出力
fp.close();/////
int BOnum=0;
for(int i=0;i<fluid_number;i++) if(PART[i].surface==ON) BOnum++;
ofstream fout2("Lorentz.fld");
fout2 << "# AVS field file" << endl;
fout2 << "ndim=1" << endl;
fout2 << "dim1=" << fluid_number <<endl;
//fout2 << "dim1=" << BOnum <<endl;
fout2 << "nspace=3" << endl;
fout2 << "veclen=3" << endl;
fout2 << "data=float" << endl;
fout2 << "field=irregular" << endl;
fout2 << "label=e-x e-y e-z" << endl << endl;
fout2 << "variable 1 file=./Lorentz filetype=ascii skip=1 offset=0 stride=6" << endl;
fout2 << "variable 2 file=./Lorentz filetype=ascii skip=1 offset=1 stride=6" << endl;
fout2 << "variable 3 file=./Lorentz filetype=ascii skip=1 offset=2 stride=6" << endl;
fout2 << "coord 1 file=./Lorentz filetype=ascii skip=1 offset=3 stride=6" << endl;
fout2 << "coord 2 file=./Lorentz filetype=ascii skip=1 offset=4 stride=6" << endl;
fout2 << "coord 3 file=./Lorentz filetype=ascii skip=1 offset=5 stride=6" << endl;
fout2.close();
ofstream fout("Lorentz");
fout<<"e-x e-y e-z x y z"<<endl;
for(int i=0;i<fluid_number;i++)
{
//if(PART[i].surface==ON)
{
fout<<F[A_X][i]*times<<" "<<F[A_Y][i]*times<<" "<<F[A_Z][i]*times<<" "<<PART[i].r[A_X]<<" "<<PART[i].r[A_Y]<<" "<<PART[i].r[A_Z]<<endl;
}
}
fout.close();
int flag=0;
if(CON->get_EM_interval()>1 &&t==1) flag=ON;
//else if(t==1 || t%10==0) flag=ON;
///////////////////////////////
if(flag==ON)
{
char filename[25];
sprintf_s(filename,"Lorentz%d.fld", t);
ofstream fout2(filename);
fout2 << "# AVS field file" << endl;
fout2 << "ndim=1" << endl;
fout2 << "dim1=" << fluid_number <<endl;
//fout2 << "dim1=" << BOnum <<endl;
fout2 << "nspace=3" << endl;
fout2 << "veclen=3" << endl;
fout2 << "data=float" << endl;
fout2 << "field=irregular" << endl;
fout2 << "label=e-x e-y e-z" << endl << endl;
fout2 << "variable 1 file=./Lorentz"<<t<<" filetype=ascii skip=1 offset=0 stride=6" << endl;
fout2 << "variable 2 file=./Lorentz"<<t<<" filetype=ascii skip=1 offset=1 stride=6" << endl;
fout2 << "variable 3 file=./Lorentz"<<t<<" filetype=ascii skip=1 offset=2 stride=6" << endl;
fout2 << "coord 1 file=./Lorentz"<<t<<" filetype=ascii skip=1 offset=3 stride=6" << endl;
fout2 << "coord 2 file=./Lorentz"<<t<<" filetype=ascii skip=1 offset=4 stride=6" << endl;
fout2 << "coord 3 file=./Lorentz"<<t<<" filetype=ascii skip=1 offset=5 stride=6" << endl;
fout2.close();
char filename2[25];
sprintf_s(filename2,"Lorentz%d", t);
ofstream fout(filename2);
fout<<"e-x e-y e-z x y z"<<endl;
for(int i=0;i<fluid_number;i++)
{
//if(PART[i].surface==ON)
{
fout<<F[A_X][i]*times<<" "<<F[A_Y][i]*times<<" "<<F[A_Z][i]*times<<" "<<PART[i].r[A_X]<<" "<<PART[i].r[A_Y]<<" "<<PART[i].r[A_Z]<<endl;
}
}
fout.close();
}
int BOnumsec=0;
for(int i=0;i<fluid_number;i++) if(PART[i].r[A_Y]>sin(PI/24)*PART[i].r[A_X]-0.5*le && PART[i].r[A_Y]<sin(PI/24)*PART[i].r[A_X]+0.5*le) BOnumsec++;
ofstream foutz("Lorentzsec.fld");
foutz << "# AVS field file" << endl;
foutz << "ndim=1" << endl;
//foutz << "dim1=" << fluid_number <<endl;
foutz << "dim1=" << BOnumsec <<endl;
foutz << "nspace=3" << endl;
foutz << "veclen=3" << endl;
foutz << "data=float" << endl;
foutz << "field=irregular" << endl;
foutz << "label=e-x e-y e-z" << endl << endl;
foutz << "variable 1 file=./Lorentzsec filetype=ascii skip=1 offset=0 stride=6" << endl;
foutz << "variable 2 file=./Lorentzsec filetype=ascii skip=1 offset=1 stride=6" << endl;
foutz << "variable 3 file=./Lorentzsec filetype=ascii skip=1 offset=2 stride=6" << endl;
foutz << "coord 1 file=./Lorentzsec filetype=ascii skip=1 offset=3 stride=6" << endl;
foutz << "coord 2 file=./Lorentzsec filetype=ascii skip=1 offset=4 stride=6" << endl;
foutz << "coord 3 file=./Lorentzsec filetype=ascii skip=1 offset=5 stride=6" << endl;
foutz.close();
ofstream fout2z("Lorentzsec");
fout2z<<"e-x e-y e-z x y z"<<endl;
for(int i=0;i<fluid_number;i++)
{
//if(PART[i].surface==ON)
//if(PART[i].r[A_Z]>0.14125-0.5*le && PART[i].r[A_Z]<0.14125+0.5*le)
if(PART[i].r[A_Y]>sin(PI/24)*PART[i].r[A_X]-0.5*le && PART[i].r[A_Y]<sin(PI/24)*PART[i].r[A_X]+0.5*le)
{
fout2z<<F[A_X][i]*times<<" "<<F[A_Y][i]*times<<" "<<F[A_Z][i]*times<<" "<<PART[i].r[A_X]<<" "<<PART[i].r[A_Y]<<" "<<PART[i].r[A_Z]<<endl;
}
}
fout2z.close();
}
int main()
{
std::ofstream fout1("test1.dat"); // Flat Sky
std::ofstream fout2("test2.dat"); // Spherical near equator
std::ofstream fout3("test3.dat"); // Spherical around N pole.
std::ofstream fout4("test4.dat"); // Test1 with random offsets
std::ofstream fout4c("test4_centers.dat"); // Corresponding centers for test4.dat
fout1.precision(12);
fout2.precision(12);
fout3.precision(12);
fout4.precision(12);
fout4c.precision(12);
double ra0 = 0.; // RA, Dec of center for test2.
double dec0 = 80.;
const long ngal = 100000;
const double rscale = 1.; // Scale radius in degrees.
const double rmax = 5.; // In units of rscale
// The functional form imprinted is purely radial:
// gamma_t(r) = gamma0/r
// kappa(r) = kappa0/r
// n(r) ~ exp(-r^2/2)
// where r is in units of rscale
const double gamma0 = 1.e-3;
const double kappa0 = 3.e-3;
// Make sure gamma_t < 0.5
const double rmin = gamma0/0.5;
srand(1234); // To make it deterministic.
// Normally for the Box-Muller Transformation, one would restrict rsq < 1
// But we then want to clip the resulting distribution at rmax, so
// rsq * fac^2 < rmax^2
// rsq * (-2.*log(rsq)/rsq) < rmax^2
// -2. * log(rsq) < rmax^2
// rsq > exp(-rmax^2/2)
const double rsqmin = exp(-rmax*rmax/2.);
const double rsqmax = exp(-rmin*rmin/2.);
//std::cout<<"rsq min/max = "<<rsqmin<<','<<rsqmax<<std::endl;
double xdeg_min=0., xdeg_max=0., var_xdeg=0.;
double ydeg_min=0., ydeg_max=0., var_ydeg=0.;
for (long n=0; n<ngal; ++n) {
double x,y,rsq;
do {
x = double(rand()) / RAND_MAX; // Random number from 0..1
y = double(rand()) / RAND_MAX;
//std::cout<<"x,y = "<<x<<','<<y<<std::endl;
x = 2.*x-1.; // Now from -1..1
y = 2.*y-1.;
//std::cout<<"x,y => "<<x<<','<<y<<std::endl;
rsq = x*x+y*y;
//std::cout<<"rsq = "<<rsq<<std::endl;
} while (rsq <= rsqmin || rsq >= rsqmax);
// Use Box-Muller Transformation to convert to Gaussian distribution in x,y.
double fac = sqrt(-2.*log(rsq)/rsq);
x *= fac;
y *= fac;
double r = fac*sqrt(rsq);
double theta = atan2(y,x);
double g = gamma0 / r;
double k = kappa0 / r;
assert(g < 0.5);
double xdeg = x*rscale;
double ydeg = y*rscale;
double rdeg = r*rscale;
// Do some sanity checks:
if (xdeg < xdeg_min) xdeg_min = xdeg;
if (xdeg > xdeg_max) xdeg_max = xdeg;
if (ydeg < ydeg_min) ydeg_min = ydeg;
if (ydeg > ydeg_max) ydeg_max = ydeg;
var_xdeg += xdeg*xdeg;
var_ydeg += ydeg*ydeg;
//
// Flat sky:
//
double g1 = -g * cos(2.*theta);
double g2 = -g * sin(2.*theta);
fout1 << xdeg <<" "<< ydeg <<" "<< g1 <<" "<< g2 <<" "<< k <<std::endl;
// With offsets in position:
double dx = 2.*double(rand()) / RAND_MAX - 1.; // Random number from -1..1
double dy = 2.*double(rand()) / RAND_MAX - 1.;
dx *= rscale;
dy *= rscale;
fout4 << xdeg + dx <<" "<< ydeg + dy <<" "<< g1 <<" "<< g2 <<" "<< k <<std::endl;
fout4c << dx <<" "<< dy <<" 1. "<<std::endl;
//
// Spherical near equator:
//
// Use spherical triangle with A = point, B = (ra0,dec0), C = N. pole
// a = Pi/2-dec0
// c = 2*atan(r/2)
// B = Pi/2 - theta
// Solve the rest of the triangle with spherical trig:
double c = 2.*atan( (rdeg*M_PI/180.) / 2.);
double a = M_PI/2. - (dec0*M_PI/180.);
double B = x > 0 ? M_PI/2. - theta : theta - M_PI/2.;
if (B < 0) B += 2.*M_PI;
if (B > 2.*M_PI) B -= 2.*M_PI;
double cosb = cos(a)*cos(c) + sin(a)*sin(c)*cos(B);
double b = std::abs(cosb) < 1. ? acos(cosb) : 0.;
double cosA = (cos(a) - cos(b)*cos(c)) / (sin(b)*sin(c));
double A = std::abs(cosA) < 1. ? acos(cosA) : 0.;
double cosC = (cos(c) - cos(a)*cos(b)) / (sin(a)*sin(b));
double C = std::abs(cosC) < 1. ? acos(cosC) : 0.;
//std::cout<<"x,y = "<<x<<','<<y<<std::endl;
//std::cout<<"a = "<<a<<std::endl;
//std::cout<<"b = "<<b<<std::endl;
//std::cout<<"c = "<<c<<std::endl;
//std::cout<<"A = "<<A<<std::endl;
//std::cout<<"B = "<<B<<std::endl;
//std::cout<<"C = "<<C<<std::endl;
// Compute ra,dec from these.
// Note: increasing x is decreasing ra. East is left on the sky!
double ra = x>0 ? -C : C;
double dec = M_PI/2. - b;
ra *= 180. / M_PI;
dec *= 180. / M_PI;
ra += ra0;
//std::cout<<"ra = "<<ra<<std::endl;
//std::cout<<"dec = "<<dec<<std::endl;
// Rotate shear relative to local west
std::complex<double> gamma(g1,g2);
double beta = M_PI - (A+B);
if (x > 0) beta = -beta;
//std::cout<<"gamma = "<<gamma<<std::endl;
//std::cout<<"beta = "<<beta<<std::endl;
std::complex<double> exp2ibeta(cos(2.*beta),sin(2.*beta));
gamma *= exp2ibeta;
//std::cout<<"gamma => "<<gamma<<std::endl;
fout2 << ra <<" "<< dec <<" "<< real(gamma) <<" "<<imag(gamma) <<" "<<k <<std::endl;
//
// Spherical around N pole
//
dec = 90. - c * 180./M_PI;
ra = theta * 12. / M_PI;
fout3 << ra <<" "<< dec <<" "<< g <<" "<<0. <<" "<<k <<std::endl;
}
var_xdeg /= ngal;
var_ydeg /= ngal;
std::cout<<"Min/Max x = "<<xdeg_min<<" "<<xdeg_max<<std::endl;;
std::cout<<"Min/Max y = "<<ydeg_min<<" "<<ydeg_max<<std::endl;;
std::cout<<"sqrt(Var(x)) = "<<sqrt(var_xdeg)<<std::endl;
std::cout<<"sqrt(Var(y)) = "<<sqrt(var_ydeg)<<std::endl;
// Make random catalogs
std::ofstream foutr1("rand1.dat");
std::ofstream foutr2("rand2.dat");
std::ofstream foutr3("rand3.dat");
foutr1.precision(12);
foutr2.precision(12);
foutr3.precision(12);
xdeg_min=xdeg_max=var_xdeg=0.;
ydeg_min=ydeg_max=var_ydeg=0.;
for (long n=0; n<10*ngal; ++n) {
double x,y,rsq;
do {
x = double(rand()) / RAND_MAX; // Random number from 0..1
y = double(rand()) / RAND_MAX;
x = 2.*x-1.; // Now from -1..1
y = 2.*y-1.;
rsq = x*x+y*y;
} while (rsq >= 1.);
x *= rmax;
y *= rmax;
double r = rmax*sqrt(rsq);
double theta = atan2(y,x);
double xdeg = x*rscale;
double ydeg = y*rscale;
double rdeg = r*rscale;
// Do some sanity checks:
if (xdeg < xdeg_min) xdeg_min = xdeg;
if (xdeg > xdeg_max) xdeg_max = xdeg;
if (ydeg < ydeg_min) ydeg_min = ydeg;
if (ydeg > ydeg_max) ydeg_max = ydeg;
var_xdeg += xdeg*xdeg;
var_ydeg += ydeg*ydeg;
//
// flat sky:
//
foutr1 << xdeg <<" "<< ydeg << std::endl;
//
// Spherical near equator:
//
double c = 2.*atan( (rdeg*M_PI/180.) / 2.);
double a = M_PI/2. - (dec0*M_PI/180.);
double B = x > 0 ? M_PI/2. - theta : theta - M_PI/2.;
if (B < 0) B += 2.*M_PI;
if (B > 2.*M_PI) B -= 2.*M_PI;
double cosb = cos(a)*cos(c) + sin(a)*sin(c)*cos(B);
double b = std::abs(cosb) < 1. ? acos(cosb) : 0.;
double cosC = (cos(c) - cos(a)*cos(b)) / (sin(a)*sin(b));
double C = std::abs(cosC) < 1. ? acos(cosC) : 0.;
double ra = x>0 ? -C : C;
double dec = M_PI/2. - b;
ra *= 180. / M_PI;
dec *= 180. / M_PI;
ra += ra0;
foutr2 << ra <<" "<< dec <<std::endl;
//
// Spherical around N pole
//
dec = 90. - c * 180./M_PI;
ra = theta * 12. / M_PI;
foutr3 << ra <<" "<< dec <<std::endl;
}
var_xdeg /= ngal;
var_ydeg /= ngal;
std::cout<<"For randoms:\n";
std::cout<<"Min/Max x = "<<xdeg_min<<" "<<xdeg_max<<std::endl;;
std::cout<<"Min/Max y = "<<ydeg_min<<" "<<ydeg_max<<std::endl;;
std::cout<<"sqrt(Var(x)) = "<<sqrt(var_xdeg)<<std::endl;
std::cout<<"sqrt(Var(y)) = "<<sqrt(var_ydeg)<<std::endl;
return 0;
}
//温度AVSファイル出力関数
void output_temperature_avs(mpsconfig *CON,vector<mpsparticle> &PART,int t,int particle_number,int fluid_number,double *T,double height)
{
char filename[30];
int n=0;
double le=CON->get_distancebp();
t=1;//いまはわざと毎ステップ上書き
//sprintf(filename,"pressure/pressure%d",t);//フォルダを作成して管理する場合はこちら
sprintf(filename,"T_XZ%d",t);//他のファイルと同じ階層に生成するならこちら
ofstream fout(filename);
if(!fout)
{
cout << "cannot open" << filename << endl;
exit(EXIT_FAILURE);
}
if(CON->get_dimention()==3)
{
for(int i=0;i<particle_number;i++)
{
if(PART[i].r[A_Y]<le && PART[i].r[A_Y]>-le)
{
double x=PART[i].r[A_X]*1.0E+05; //rは非常に小さい値なので10^5倍しておく
double y=PART[i].r[A_Y]*1.0E+05;
double z=PART[i].r[A_Z]*1.0E+05;
double P=T[i];
fout << P << "\t" << x << "\t" << y << "\t" << z << endl;
n++;
}
}
}
else if(CON->get_dimention()==2)
{
for(int i=0;i<particle_number;i++)
{
double x=PART[i].r[A_X]*1.0E+05; //rは非常に小さい値なので10^5倍しておく
double y=PART[i].r[A_Y]*1.0E+05;
double z=PART[i].r[A_Z]*1.0E+05;
double P=T[i];
fout << P << "\t" << x << "\t" << y << "\t" << z << endl;
n++;
}
}
fout.close();
//sprintf(filename,"pressure/pressure%d.fld",t);//フォルダを作成して管理する場合はこちら
sprintf(filename,"T_XZ%d.fld",t);//他のファイルと同じ階層に生成するならこちら
ofstream fout2(filename);
if(!fout2)
{
cout << "cannot open" << filename << endl;
exit(EXIT_FAILURE);
}
fout2 << "# AVS field file" << endl;
fout2 << "ndim=1" << endl;
fout2 << "dim1=" << n <<endl;
fout2 << "nspace=3" << endl;
fout2 << "veclen=1" << endl;
fout2 << "data=float" << endl;
fout2 << "field=irregular" << endl;
fout2 << "label=temperature" << endl << endl;
//fout2 << "variable 1 file=./pressure" << t << " " << "filetype=ascii offset=0 stride=4" << endl;//フォルダを作成して管理する場合はこちら
//fout2 << "coord 1 file=./pressure" << t << " " << "filetype=ascii offset=1 stride=4" << endl;//フォルダを作成して管理する場合はこちら
//fout2 << "coord 2 file=./pressure" << t << " " << "filetype=ascii offset=2 stride=4" << endl;//フォルダを作成して管理する場合はこちら
//fout2 << "coord 3 file=./pressure" << t << " " << "filetype=ascii offset=3 stride=4" << endl;//フォルダを作成して管理する場合はこちら
fout2 << "variable 1 file=T_XZ" << t << " " << "filetype=ascii offset=0 stride=4" << endl;//他のファイルと同じ階層に生成するならこちら
fout2 << "coord 1 file=T_XZ" << t << " " << "filetype=ascii offset=1 stride=4" << endl;//他のファイルと同じ階層に生成するならこちら
fout2 << "coord 2 file=T_XZ" << t << " " << "filetype=ascii offset=2 stride=4" << endl;//他のファイルと同じ階層に生成するならこちら
fout2 << "coord 3 file=T_XZ" << t << " " << "filetype=ascii offset=3 stride=4" << endl;//他のファイルと同じ階層に生成するならこちら
fout2.close();
}
int
main(int argc,char** argv){
quality=atof(argv[1]);
BMP fin2 (argv[2]);
BMP fin ;fin.init(fin2.w,fin2.h);
BMP fout2;fout2.init(fin2.w,fin2.h);
BMP fout;fout.init(fin2.w,fin2.h);
double in[8][8];
double out[8][8];
double out2[8][8];
int i,j,idx;
init_walsh();
init_dwt();
bmp_for(fin)
RGB2YUV(fin2(x,y),fin(x,y));
int num_dat[14];
double prob_dat[14];
int sum_dat[14];
double bit_dat[14];
memset(num_dat,0,sizeof(num_dat));
int num_totallen=0;
for(int c=0;c<3;c++){
for(int by=0;by<fin.h/8;by++)
for(int bx=0;bx<fin.w/8;bx++){
int num_codelen=0;
for(int dy=0;dy<8;dy++)
for(int dx=0;dx<8;dx++){
int x=bx*8+dx;
int y=by*8+dy;
in[ dy][ dx]=fin(x,y)[c];
// in[ dy][ dx]=255;
}
//dct(in,out);
//walsh(in,out);
// mprint(in);
//dwt(in,out);
haar(in,out);
// mprint(out);
// idwt(out,in);
// mprint(in);
// return 0;
// ihaar(out,in);
// mprint(in);
// return 0;
// if(c!=0)
mmap(out,(1.0/(qt[c][y][x]*quality))*);
mmap(out,round);
// mprint(out);
vector<RunBit> rvec;
vector<Code> cvec;
mkRunBit(out,rvec);
mkCode(rvec,cvec);
for(int dy=0;dy<8;dy++)
for(int dx=0;dx<8;dx++){
num_dat[(int)(log(fabs(out[dy][dx])+1.0)/log(2))]++;
}
// for(int i=0;i<rvec.size();i++)
// printf("%d %d\n",rvec[i].zero,rvec[i].code);
// printf("\n");
// for(int i=0;i<cvec.size();i++)
// printf("%d %x\n",cvec[i].len,cvec[i].code);
// return 0;
for(int i=0;i<cvec.size();i++)
num_codelen+=cvec[i].len;
double comprate=(double)(8*8*8)/(double)(num_codelen);
// if(comprate<3){
// mprint(out);
// for(int i=0;i<rvec.size();i++)
// printf("%d %d\n",rvec[i].zero,rvec[i].code);
// printf("\n");
// for(int i=0;i<cvec.size();i++)
// printf("%d %x\n",cvec[i].len,cvec[i].code);
// printf("\n");
// printf("comprate %f",comprate);
// return 0;
// }
num_totallen+=num_codelen;
// if(c!=0)
mmap(out,(qt[c][y][x]*quality)*);
//idct(out,out2);
//walsh(out,out2);
//idwt(out,out2);
ihaar(out,out2);
for(int dy=0;dy<8;dy++)
for(int dx=0;dx<8;dx++){
int x=bx*8+dx;
int y=by*8+dy;
fout(x,y)[c]=lmt3[c](out2[ dy][ dx]);
}
}
}
bmp_for(fin)
YUV2RGB(fout(x,y),fout2(x,y));
for(int y=0;y<14;y++)
prob_dat[y]=(double)(num_dat[y])/(double)(fin.w*fin.h*3);
for(int y=0;y<14;y++)
sum_dat[y]=1<<y;
for(int y=0;y<14;y++)
if(sum_dat[y]!=0&&prob_dat[y]!=0.0)
bit_dat[y]=-log(prob_dat[y]/sum_dat[y])/log(2);
double ave_bits=0;
for(int y=0;y<14;y++)
ave_bits+=prob_dat[y]*bit_dat[y];
for(int y=0;y<14;y++){
printf("%2d,%5d,%9d,%8.3f,%8.3f,%8.3f\n",
y,
sum_dat[y],
num_dat[y],
prob_dat[y],
bit_dat[y],
prob_dat[y]*bit_dat[y]
);
}
printf("ideal comprate:%f\n",8.0/ave_bits);
double tcomprate=(double)(fin.w*fin.h*24)/(double)(num_totallen);
printf("tcomprate %f\n",tcomprate);
fout2.write(argv[3]);
return true;
}
void fitwitherr2() {
fetchpc3dphidz();
float pt[50];
float errx[50];
for (int ipt=0; ipt<50; ipt++) {
pt[ipt] = ipt*0.1 + 0.05;
errx[ipt] = 0.0;
}
gStyle->SetOptStat(kFALSE);
//gStyle->SetOptFit(1101);
ofstream fout("run16dAupc3matchingsmooth.h");
ofstream fout2("run16dAupc3matchingsmooth2.h");
for (int iarm=0; iarm<2; iarm++) {
for (int ich=0; ich<2; ich++) {
for(int ivz=0; ivz<10; ivz++){
string arm;
string ch;
if (iarm == 0) arm = "east";
else if (iarm == 1) arm = "west";
else arm = "err";
if (ich == 0) ch = "pos";
else if (ich == 1) ch = "neg";
else ch = "err";
string bbcz;
if (ivz==0) bbcz = "-10 to -8";
else if (ivz==1) bbcz = "-8 to -6";
else if (ivz==2) bbcz = "-6 to -4";
else if (ivz==3) bbcz = "-4 to -2";
else if (ivz==4) bbcz = "-2 to 0";
else if (ivz==5) bbcz = "0 to 2";
else if (ivz==6) bbcz = "2 to 4";
else if (ivz==7) bbcz = "4 to 6";
else if (ivz==8) bbcz = "6 to 8";
else if (ivz==9) bbcz = "8 to 10";
else bbcz = "err";
TCanvas *c1 = new TCanvas("c1","c1",500,500);
c1->SetGridx();
TGraphErrors* dphisigma = new TGraphErrors(50,pt,pc3dphisigma[iarm][ich][ivz],errx,pc3dphisigmaerr[iarm][ich][ivz]);
dphisigma->SetTitle(Form("dphisigma_%s_%s_%s",arm,ch,bbcz));
dphisigma->SetLineColor(1);
dphisigma->SetMarkerStyle(20);
dphisigma->SetMarkerSize(0.8);
dphisigma->GetXaxis()->SetRangeUser(0.2,5.0);
dphisigma->GetHistogram()->SetMaximum(0.01);
dphisigma->GetHistogram()->SetMinimum(-0.01);
dphisigma->GetXaxis()->SetTitle("p_{T}");
dphisigma->GetYaxis()->SetTitle("dphi sigma");
dphisigma->Draw("AP");
TPaveText *p1 = new TPaveText(0.2,0.8,0.9,0.9,"NDC");
p1->AddText("Fit function: a+bx+cx^{2}+dx^{3}+ex^{4}+fx^{5}+#frac{g}{#sqrt{x}}+#frac{h}{x^{2}}");
p1->Draw("same");
TF1 *fdphisigma = new TF1("fdphisigma","[0]+[1]*x+[2]*x*x+[3]*x*x*x+[4]*x*x*x*x+[5]*x*x*x*x*x+[6]/TMath::Sqrt(x)+[7]/x/x",0.3,5.0);
//TF1 *fdphisigma2 = new TF1("fdphisigma2","[0]+[1]*x+[2]*x*x+[3]*x*x*x+[4]*x*x*x*x+[5]*x*x*x*x*x+[6]/TMath::Sqrt(x)+[7]/x/x",0.2,5.0);
dphisigma->Fit("fdphisigma","Q0","",0.4,4.0);
//double sigma_par[8];
//fdphisigma->GetParameters(sigma_par);
//fdphisigma->SetParameters(sigma_par);
//dphisigma->Fit("fdphisigma2","Q0","",0.3,5.0);
fdphisigma->Draw("same");
fout << "fdphisigma->SetParameters(" << fdphisigma->GetParameter(0) << "," << fdphisigma->GetParameter(1) << "," << fdphisigma->GetParameter(2) << "," << fdphisigma->GetParameter(3) << "," << fdphisigma->GetParameter(4) << "," << fdphisigma->GetParameter(5) << "," << fdphisigma->GetParameter(6) << "," << fdphisigma->GetParameter(7) << ");" << endl;
for(int ipar=0;ipar<8;ipar++){
fout2 << "PC3_dphisigma[" << iarm <<"][" << ich << "][" << ivz <<"][" << ipar << "] = " << fdphisigma->GetParameter(ipar) << ";" << endl;
}
c1->Print(Form("smooth/dphisigma_%d_%d_%d.png",iarm,ich,ivz));
delete c1;
TCanvas *c3 = new TCanvas("c3","c3",500,500);
c3->SetGridx();
TGraphErrors* dphimean = new TGraphErrors(50,pt,pc3dphimean[iarm][ich][ivz],errx,pc3dphimeanerr[iarm][ich][ivz]);
dphimean->SetTitle(Form("dphimean_%s_%s_%s",arm,ch,bbcz));
dphimean->SetLineColor(1);
dphimean->SetMarkerStyle(20);
dphimean->SetMarkerSize(0.8);
dphimean->GetXaxis()->SetRangeUser(0.2,5.0);
dphimean->GetHistogram()->SetMaximum(0.01);
dphimean->GetHistogram()->SetMinimum(-0.01);
dphimean->GetXaxis()->SetTitle("p_{T}");
dphimean->GetYaxis()->SetTitle("dphi mean");
dphimean->Draw("AP");
TPaveText *p3 = new TPaveText(0.2,0.8,0.9,0.9,"NDC");
p3->AddText("Fit function: a+bx+#frac{c}{x}+#frac{d}{#sqrt{x}}+#frac{e}{x^{2}}+#frac{f}{x^{3}}+#frac{g}{x^{4}}");
p3->Draw("same");
TF1 *fdphimean = new TF1("fdphimean","[0]+[1]*x+[2]/x+[3]/TMath::Sqrt(x)+[4]/x/x+[5]/x/x/x+[6]/x/x/x/x",0.3,5.0);
//TF1 *fdphimean2 = new TF1("fdphimean2","[0]+[1]*x+[2]/x+[3]/TMath::Sqrt(x)+[4]/x/x+[5]/x/x/x+[6]/x/x/x/x",0.2,5.0);
dphimean->Fit("fdphimean","Q0","",0.4,4.0);
double mean_par[7];
//fdphimean->GetParameters(mean_par);
//fdphimean2->SetParameters(mean_par);
//dphimean->Fit("fdphimean2","Q0","",0.3,5.0);
fdphimean->Draw("same");
fout << "fdphimean->SetParameters(" << fdphimean->GetParameter(0) << "," << fdphimean->GetParameter(1) << "," << fdphimean->GetParameter(2) << "," << fdphimean->GetParameter(3) << "," << fdphimean->GetParameter(4) << "," << fdphimean->GetParameter(5) << "," << fdphimean->GetParameter(6) << ");" << endl;
for(int ipar=0;ipar<7;ipar++){
fout2 << "PC3_dphimean[" << iarm <<"][" << ich << "][" << ivz <<"][" << ipar << "] = " << fdphimean->GetParameter(ipar) << ";" << endl;
}
c3->Print(Form("smooth/dphimean_%d_%d_%d.png",iarm,ich,ivz));
delete c3;
TCanvas *c2 = new TCanvas("c2","c2",500,500);
c2->SetGridx();
TGraphErrors* dzsigma = new TGraphErrors(50,pt,pc3dzsigma[iarm][ich][ivz],errx,pc3dzsigmaerr[iarm][ich][ivz]);
dzsigma->SetTitle(Form("dzsigma_%s_%s_%s",arm,ch,bbcz));
dzsigma->SetLineColor(1);
dzsigma->SetMarkerStyle(20);
dzsigma->SetMarkerSize(0.8);
dzsigma->GetXaxis()->SetRangeUser(0.2,5.0);
dzsigma->GetHistogram()->SetMaximum(5);
dzsigma->GetHistogram()->SetMinimum(0);
dzsigma->GetXaxis()->SetTitle("p_{T}");
dzsigma->GetYaxis()->SetTitle("dz sigma");
dzsigma->Draw("AP");
TPaveText *p2 = new TPaveText(0.2,0.8,0.9,0.9,"NDC");
p2->AddText("Fit function: a+bx+cx^{2}+dx^{3}+ex^{4}+fx^{5}+#frac{g}{#sqrt{x}}+#frac{h}{x^{2}}");
p2->Draw("same");
//TF1 *fdzsigma = new TF1("fdzsigma","[0]+[1]*x+[2]*x*x+[3]*x*x*x+[4]*x*x*x*x+[5]/TMath::Sqrt(x)+[6]/x/x",0.3,5.0);
TF1 *fdzsigma = new TF1("fdzsigma","[0]+[1]*x+[2]*x*x+[3]*x*x*x+[4]*x*x*x*x+[5]*x*x*x*x*x+[6]/TMath::Sqrt(x)+[7]/x/x",0.3,5.0);
//TF1 *fdzsigma2 = new TF1("fdzsigma2","[0]+[1]*x+[2]*x*x+[3]*x*x*x+[4]*x*x*x*x+[5]/TMath::Sqrt(x)+[6]/x/x",0.2,5.0);
dzsigma->Fit("fdzsigma","Q0","",0.4,4.0);
//fdzsigma->GetParameters(sigma_par);
//fdzsigma2->SetParameters(sigma_par);
//dzsigma->Fit("fdzsigma2","Q0","",0.3,5.0);
fdzsigma->Draw("same");
fout << "fdzsigma->SetParameters(" << fdzsigma->GetParameter(0) << "," << fdzsigma->GetParameter(1) << "," << fdzsigma->GetParameter(2) << "," << fdzsigma->GetParameter(3) << "," << fdzsigma->GetParameter(4) << "," << fdzsigma->GetParameter(5) << "," << fdzsigma->GetParameter(6) << ");" << endl;
for(int ipar=0;ipar<8;ipar++){
fout2 << "PC3_dzsigma[" << iarm <<"][" << ich << "][" << ivz <<"][" << ipar << "] = " << fdzsigma->GetParameter(ipar) << ";" << endl;
}
c2->Print(Form("smooth/dzsigma_%d_%d_%d.png",iarm,ich,ivz));
delete c2;
TCanvas *c4 = new TCanvas("c4","c4",500,500);
c4->SetGridx();
TGraphErrors* dzmean = new TGraphErrors(50,pt,pc3dzmean[iarm][ich][ivz],errx,pc3dzmeanerr[iarm][ich][ivz]);
dzmean->SetTitle(Form("dzmean_%s_%s_%s",arm,ch,bbcz));
dzmean->SetLineColor(1);
dzmean->SetMarkerStyle(20);
dzmean->SetMarkerSize(0.8);
dzmean->GetXaxis()->SetRangeUser(0.2,5.0);
dzmean->GetHistogram()->SetMaximum(3);
dzmean->GetHistogram()->SetMinimum(-2);
dzmean->GetXaxis()->SetTitle("p_{T}");
dzmean->GetYaxis()->SetTitle("dz mean");
dzmean->Draw("AP");
TPaveText *p4 = new TPaveText(0.2,0.8,0.9,0.9,"NDC");
p4->AddText("Fit function: a+bx+#frac{c}{x}+#frac{d}{#sqrt{x}}+#frac{e}{x^{2}}+#frac{f}{x^{3}}+#frac{g}{x^{4}}");
p4->Draw("same");
TF1 *fdzmean = new TF1("fdzmean","[0]+[1]*x+[2]/x+[3]/TMath::Sqrt(x)+[4]/x/x+[5]/x/x/x+[6]/x/x/x/x",0.3,5.0);
//TF1 *fdzmean2 = new TF1("fdzmean2","[0]+[1]*x+[2]/x+[3]/TMath::Sqrt(x)+[4]/x/x+[5]/x/x/x+[6]/x/x/x/x",0.2,5.0);
dzmean->Fit("fdzmean","Q0","",0.4,4.0);
//fdzmean->GetParameters(mean_par);
//fdzmean2->SetParameters(mean_par);
//dzmean->Fit("fdzmean2","Q0","",0.3,5.0);
fdzmean->Draw("same");
fout << "fdzmean->SetParameters(" << fdzmean->GetParameter(0) << "," << fdzmean->GetParameter(1) << "," << fdzmean->GetParameter(2) << "," << fdzmean->GetParameter(3) << "," << fdzmean->GetParameter(4) << "," << fdzmean->GetParameter(5) << "," << fdzmean->GetParameter(6) << ");" << endl;
for(int ipar=0;ipar<7;ipar++){
fout2 << "PC3_dzmean[" << iarm <<"][" << ich << "][" << ivz <<"][" << ipar << "] = " << fdzmean->GetParameter(ipar) << ";" << endl;
}
c4->Print(Form("smooth/dzmean_%d_%d_%d.png",iarm,ich,ivz));
delete c4;
}
}
}
}
void tracking(string thestring){
//string thestring = "Position_REF2X_42_48";
string txtfilename = thestring + ".txt";
string shiftHead = "RotationBack_shiftParameters_";
string rotateHead = "RotationBack_angles_";
string residualHead = "RotationBack_residuals_";
string ResidualRHead="RotationBack_Residual_";
string chi2Head = "ResolutionChi2Angle_";
string foutname = shiftHead+thestring+"_exclusive.txt";
string fout1name = residualHead+thestring+"_exclusive.txt";
string foutchi2name = chi2Head + thestring + "_exclusive.txt";
string foutRotationName = rotateHead + thestring + "_exclusive.txt";
fstream fin(txtfilename.c_str(),ios::in);
if(!fin){cout<<"file not read"<<endl; return;}
else cout<<"processing "<<txtfilename<<endl;
fstream fout(foutname.c_str(),ios::out);
fstream fout1(fout1name.c_str(),ios::out);
fstream fout2(foutRotationName.c_str(),ios::out);
fstream fout3(foutchi2name.c_str(),ios::out|ios::app);
double pREF2X=0.0, pREF2Y=0.0;
double pREF3X=0.0, pREF3Y=0.0;
double pUVA3X=0.0, pUVA3Y=0.0;
double pREF1X=0.0, pREF1Y=0.0;
//double pZZ1=0.0, pZZ2=0.0;
//double pEta5=0.0;
vector<double> vpREF2X; vector<double> vpREF2Y;
vector<double> vpREF3X; vector<double> vpREF3Y;
vector<double> vpUVA3X; vector<double> vpUVA3Y;
vector<double> vpREF1X; vector<double> vpREF1Y;
//vector<double> vpZZ1; vector<double> vpZZ2;
//vector<double> vpEta5;
Int_t nbLines=0;
while(fin>>pREF2X>>pREF2Y>>pREF3X>>pREF3Y>>pUVA3X>>pUVA3Y>>pREF1X>>pREF1Y/*>>pEta5*/){
vpREF2X.push_back(pREF2X); vpREF2Y.push_back(pREF2Y); vpREF3X.push_back(pREF3X); vpREF3Y.push_back(pREF3Y);
vpUVA3X.push_back(pUVA3X); vpUVA3Y.push_back(pUVA3Y); vpREF1X.push_back(pREF1X); vpREF1Y.push_back(pREF1Y);
//vpZZ1.push_back(pZZ1); vpZZ2.push_back(pZZ2);
//vpEta5.push_back(pEta5);
nbLines++;
}
fin.close();
/*
REF2X:
[-30,-20]: -22.98, 1.089, -28.29, -3.209, -37.22, 14.88, -46.62, -2.411, 29.84
[-20,-15]: -17.07, 0.4154, -22.27, -3.92, -31.12, 14.55, -40.55, -2.637, 29.48
*/
/* coarse alignment parameters
double shiREF2X=10.72, shiREF2Y=1.048;
double shiREF3X=6.162, shiREF3Y=-3.395;
double shiUVA3X=-1.489, shiUVA3Y=15.05;
double shiREF1X=-10.46, shiREF1Y=-1.496;
//double shiZZ1=0, shiZZ2=0;
double shiEta5=29.5;
*/
//fine alignment parameters
/*
double shiREF2X=3.656, shiREF2Y=1.865;
double shiREF3X=-1.781, shiREF3Y=-2.415;
double shiUVA3X=-8.389, shiUVA3Y=15.779;
double shiREF1X=-17.504, shiREF1Y=-0.844;
double aREF3REF2=0.009499;
double aUVA3REF2=-0.004731;
double aREF1REF2=-0.02473;
*/
double shiREF2X=12.26791212, shiREF2Y=2.02708225;
double shiREF3X=7.4824985, shiREF3Y=-2.35562;
double shiUVA3X=-0.7097, shiUVA3Y=15.75778;
double shiREF1X=-9.92822, shiREF1Y=-0.68756;
double shiEta5=29.47;
double aREF3REF2=-0.02;//0.0037;//0.01254602;//start angle: 0.01197;
double aUVA3REF2=-0.004971;//-0.017;//0.00203396;//0.02807;//start angle: 0.01102
double aREF1REF2=-0.02446;//-0.0486;//-0.0171;//start angle: 0.008273;
//double aEta5REF2;
double tempREF2X, tempREF2Y, tempREF3X, tempREF3Y, tempUVA3X, tempUVA3Y, tempREF1X, tempREF1Y, tempEta5;
double meanREF2X=0.0, meanREF2Y=0.0;
double meanREF3X=0.0, meanREF3Y=0.0;
double meanUVA3X=0.0, meanUVA3Y=0.0;
double meanREF1X=0.0, meanREF1Y=0.0;
//double meanZZ1=0.0, meanZZ2=0.0;
//double meanEta5=0;
double meanAngleREF3=0.0;
double meanAngleUVA3=0.0;
double meanAngleREF1=0.0;
//double meanAngleEta5=0.0;
double sigmaREF2X=0.0,sigmaREF2Y=0.0,sigmaREF3X=0.0,sigmaREF3Y=0.0,sigmaUVA3X=0.0,sigmaUVA3Y=0.0,sigmaREF1X=0.0,sigmaREF1Y=0.0;
double totalAngleREF3=0.0, totalAngleUVA3=0.0, totalAngleREF1=0.0;
double meanXChi2=0.0,meanYChi2=0.0; // chi square for tracks.
Int_t iterNb=0;
while(1){
char rootfile[50]; sprintf(rootfile,"_iter%i_exclusive.root",iterNb);
string outputrootname=ResidualRHead+thestring+rootfile;
TFile* f = new TFile(outputrootname.c_str(),"recreate");
iterNb++;
char name2X[15];sprintf(name2X,"posREF2X_%i",iterNb); char name2Y[15];sprintf(name2Y,"posREF2Y_%i",iterNb);
char name3X[15];sprintf(name3X,"posREF3X_%i",iterNb); char name3Y[15];sprintf(name3Y,"posREF3Y_%i",iterNb);
char nameu3X[15];sprintf(nameu3X,"posUVA3X_%i",iterNb);char nameu3Y[15];sprintf(nameu3Y,"posUVA3Y_%i",iterNb);
char name1X[15];sprintf(name1X,"posREF1X_%i",iterNb); char name1Y[15];sprintf(name1Y,"posREF1Y_%i",iterNb);
//char nameZZ1[15];sprintf(nameZZ1,"pos10cmZZ1_%i",iterNb); char nameZZ2[15];sprintf(nameZZ2,"pos10cmZZ2_%i",iterNb);
//char nameEta5[15];sprintf(nameEta5,"posEta5_%i",iterNb);
TH1F* hpREF2X = new TH1F(name2X,"",500,-50,50); hpREF2X->SetXTitle("mm"); hpREF2X->SetYTitle("Frequency");hpREF2X->SetLabelSize(0.045,"XY");hpREF2X->SetTitleSize(0.045,"XY");
TH1F* hpREF2Y = new TH1F(name2Y,"",500,-50,50); hpREF2Y->SetXTitle("mm"); hpREF2Y->SetYTitle("Frequency");hpREF2Y->SetLabelSize(0.045,"XY");hpREF2Y->SetTitleSize(0.045,"XY");
TH1F* hpREF3X = new TH1F(name3X,"",500,-50,50); hpREF3X->SetXTitle("mm"); hpREF3X->SetYTitle("Frequency");hpREF3X->SetLabelSize(0.045,"XY");hpREF3X->SetTitleSize(0.045,"XY");
TH1F* hpREF3Y = new TH1F(name3Y,"",500,-50,50); hpREF3Y->SetXTitle("mm"); hpREF3Y->SetYTitle("Frequency");hpREF3Y->SetLabelSize(0.045,"XY");hpREF3Y->SetTitleSize(0.045,"XY");
TH1F* hpUVA3X = new TH1F(nameu3X,"",500,-50,50); hpUVA3X->SetXTitle("mm"); hpUVA3X->SetYTitle("Frequency");hpUVA3X->SetLabelSize(0.045,"XY");hpUVA3X->SetTitleSize(0.045,"XY");
TH1F* hpUVA3Y = new TH1F(nameu3Y,"",500,-50,50); hpUVA3Y->SetXTitle("mm"); hpUVA3Y->SetYTitle("Frequency");hpUVA3Y->SetLabelSize(0.045,"XY");hpUVA3Y->SetTitleSize(0.045,"XY");
TH1F* hpREF1X = new TH1F(name1X,"",500,-50,50); hpREF1X->SetXTitle("mm"); hpREF1X->SetYTitle("Frequency");hpREF1X->SetLabelSize(0.045,"XY");hpREF1X->SetTitleSize(0.045,"XY");
TH1F* hpREF1Y = new TH1F(name1Y,"",500,-50,50); hpREF1Y->SetXTitle("mm"); hpREF1Y->SetYTitle("Frequency");hpREF1Y->SetLabelSize(0.045,"XY");hpREF1Y->SetTitleSize(0.045,"XY");
//TH1F* hpZZ1 = new TH1F(nameZZ1,"",600,-150,150); hpZZ1->SetXTitle("mm"); hpZZ1->SetYTitle("Frequency"); hpZZ1->SetLabelSize(0.045,"XY");hpZZ1->SetTitleSize(0.045,"XY");
//TH1F* hpZZ2 = new TH1F(nameZZ2,"",600,-150,150); hpZZ2->SetXTitle("mm"); hpZZ2->SetYTitle("Frequency"); hpZZ2->SetLabelSize(0.045,"XY");hpZZ2->SetTitleSize(0.045,"XY");
//TH1F* hpEta5 = new TH1F(nameEta5,"",600,-150,150); hpEta5->SetXTitle("mm"); hpEta5->SetYTitle("Frequency"); hpEta5->SetLabelSize(0.045,"XY");hpEta5->SetTitleSize(0.045,"XY");
char nameRes2X[20];sprintf(nameRes2X,"residualREF2X_%i",iterNb);char nameRes2Y[20];sprintf(nameRes2Y,"residualREF2Y_%i",iterNb);
char nameRes3X[20];sprintf(nameRes3X,"residualREF3X_%i",iterNb);char nameRes3Y[20];sprintf(nameRes3Y,"residualREF3Y_%i",iterNb);
char nameResu3X[20];sprintf(nameResu3X,"residualUVA3X_%i",iterNb);char nameResu3Y[20];sprintf(nameResu3Y,"residualUVA3Y_%i",iterNb);
char nameRes1X[20];sprintf(nameRes1X,"residualREF1X_%i",iterNb);char nameRes1Y[20];sprintf(nameRes1Y,"residualREF1Y_%i",iterNb);
//char nameResZZ1[20];sprintf(nameResZZ1,"residualZZ1_%i",iterNb);char nameResZZ2[20];sprintf(nameResZZ2,"residualZZ2_%i",iterNb);
//char nameResEta5[20];sprintf(nameResEta5,"residualEta5_%i",iterNb);
TH1F* residualREF2X = new TH1F(nameRes2X,"",200,-2,2); residualREF2X->SetXTitle("Residual [mm]"); residualREF2X->SetYTitle("Frequency");residualREF2X->SetLabelSize(0.045,"XY");residualREF2X->SetTitleSize(0.045,"XY");
TH1F* residualREF2Y = new TH1F(nameRes2Y,"",200,-2,2); residualREF2Y->SetXTitle("Residual [mm]"); residualREF2Y->SetYTitle("Frequency");residualREF2Y->SetLabelSize(0.045,"XY");residualREF2Y->SetTitleSize(0.045,"XY");
TH1F* residualREF3X = new TH1F(nameRes3X,"",200,-2,2); residualREF3X->SetXTitle("Residual [mm]"); residualREF3X->SetYTitle("Frequency");residualREF3X->SetLabelSize(0.045,"XY");residualREF3X->SetTitleSize(0.045,"XY");
TH1F* residualREF3Y = new TH1F(nameRes3Y,"",200,-2,2); residualREF3Y->SetXTitle("Residual [mm]"); residualREF3Y->SetYTitle("Frequency");residualREF3Y->SetLabelSize(0.045,"XY");residualREF3Y->SetTitleSize(0.045,"XY");
TH1F* residualUVA3X = new TH1F(nameResu3X,"",200,-2,2); residualUVA3X->SetXTitle("Residual [mm]"); residualUVA3X->SetYTitle("Frequency");residualUVA3X->SetLabelSize(0.045,"XY");residualUVA3X->SetTitleSize(0.045,"XY");
TH1F* residualUVA3Y = new TH1F(nameResu3Y,"",200,-2,2); residualUVA3Y->SetXTitle("Residual [mm]"); residualUVA3Y->SetYTitle("Frequency");residualUVA3Y->SetLabelSize(0.045,"XY");residualUVA3Y->SetTitleSize(0.045,"XY");
TH1F* residualREF1X = new TH1F(nameRes1X,"",200,-2,2); residualREF1X->SetXTitle("mm"); residualREF1X->SetYTitle("Frequency");residualREF1X->SetLabelSize(0.045,"XY");residualREF1X->SetTitleSize(0.045,"XY");
TH1F* residualREF1Y = new TH1F(nameRes1Y,"",200,-2,2); residualREF1Y->SetXTitle("mm"); residualREF1Y->SetYTitle("Frequency");residualREF1Y->SetLabelSize(0.045,"XY");residualREF1Y->SetTitleSize(0.045,"XY");
//TH1F* residualZZ1 = new TH1F(nameResZZ1,"",320,-16,16); residualZZ1->SetXTitle("Residual [mm]"); residualZZ1->SetYTitle("Frequency");residualZZ1->SetLabelSize(0.045,"XY");residualZZ1->SetTitleSize(0.045,"XY");
//TH1F* residualZZ2 = new TH1F(nameResZZ2,"",320,-16,16); residualZZ2->SetXTitle("Residual [mm]"); residualZZ2->SetYTitle("Frequency");residualZZ2->SetLabelSize(0.045,"XY");residualZZ2->SetTitleSize(0.045,"XY");
//TH1F* residualEta5 = new TH1F(nameResEta5,"",320,-16,16); residualEta5->SetXTitle("Residual [mm]"); residualEta5->SetYTitle("Frequency");residualEta5->SetLabelSize(0.045,"XY");residualEta5->SetTitleSize(0.045,"XY");
/*
char nameDxREF3X[20]; sprintf(nameDxREF3X,"REF3X_REF2X_%i",iterNb); char nameDyREF3Y[20]; sprintf(nameDyREF3Y,"REF3Y_REF2Y_%i",iterNb);
char nameDxUVA3X[20]; sprintf(nameDxUVA3X,"UVA3X_REF2X_%i",iterNb); char nameDyUVA3Y[20]; sprintf(nameDyUVA3Y,"UVA3Y_REF2Y_%i",iterNb);
char nameDxREF1X[20]; sprintf(nameDxREF1X,"REF1X_REF2X_%i",iterNb); char nameDyREF1Y[20]; sprintf(nameDyREF1Y,"REF1Y_REF2Y_%i",iterNb);
TH1F* dxREF3X = new TH1F(nameDxREF3X,"",400,-4,4); dxREF3X->SetXTitle("Delta_X_REF3X_REF2X [mm]"); dxREF3X->SetYTitle("Frequency");dxREF3X->SetLabelSize(0.045,"XY"); dxREF3X->SetTitleSize(0.045,"XY");
TH1F* dxUVA3X = new TH1F(nameDxUVA3X,"",400,-4,4); dxUVA3X->SetXTitle("Delta_X_UVA3X_REF2X [mm]"); dxUVA3X->SetYTitle("Frequency");dxUVA3X->SetLabelSize(0.045,"XY"); dxUVA3X->SetTitleSize(0.045,"XY");
TH1F* dxREF1X = new TH1F(nameDxREF1X,"",400,-4,4); dxREF1X->SetXTitle("Delta_X_REF1X_REF2X [mm]"); dxREF1X->SetYTitle("Frequency");dxREF1X->SetLabelSize(0.045,"XY"); dxREF1X->SetTitleSize(0.045,"XY");
TH1F* dyREF3Y = new TH1F(nameDyREF3Y,"",400,-4,4); dyREF3Y->SetXTitle("Delta_Y_REF3Y_REF2Y [mm]"); dyREF3Y->SetYTitle("Frequency");dyREF3Y->SetLabelSize(0.045,"XY"); dyREF3Y->SetTitleSize(0.045,"XY");
TH1F* dyUVA3Y = new TH1F(nameDyUVA3Y,"",400,-4,4); dyUVA3Y->SetXTitle("Delta_Y_UVA3Y_REF2Y [mm]"); dyUVA3Y->SetYTitle("Frequency");dyUVA3Y->SetLabelSize(0.045,"XY"); dyUVA3Y->SetTitleSize(0.045,"XY");
TH1F* dyREF1Y = new TH1F(nameDyREF1Y,"",400,-4,4); dyREF1Y->SetXTitle("Delta_Y_REF1Y_REF2Y [mm]"); dyREF1Y->SetYTitle("Frequency");dyREF1Y->SetLabelSize(0.045,"XY"); dyREF1Y->SetTitleSize(0.045,"XY");
char nameDxUVA3REF3[20]; sprintf(nameDxUVA3REF3,"UVA3X_REF3X_%i",iterNb);
char nameDxREF1UVA3[20]; sprintf(nameDxREF1UVA3,"REF1X_UVA3X_%i",iterNb);
char nameDxREF1REF3[20]; sprintf(nameDxREF1REF3,"REF1X_REF3X_%i",iterNb);
char nameDyUVA3REF3[20]; sprintf(nameDyUVA3REF3,"UVA3Y_REF3Y_%i",iterNb);
char nameDyREF1UVA3[20]; sprintf(nameDyREF1UVA3,"REF1Y_UVA3Y_%i",iterNb);
char nameDyREF1REF3[20]; sprintf(nameDyREF1REF3,"REF1Y_REF3Y_%i",iterNb);
TH1F* dxUVA3REF3 = new TH1F(nameDxUVA3REF3,"",400,-4,4); dxUVA3REF3->SetXTitle("Delta_X_UVA3X_REF3X [mm]"); dxUVA3REF3->SetYTitle("Frequency"); dxUVA3REF3->SetLabelSize(0.045,"XY"); dxUVA3REF3->SetTitleSize(0.045,"XY");
TH1F* dxREF1UVA3 = new TH1F(nameDxREF1UVA3,"",400,-4,4); dxREF1UVA3->SetXTitle("Delta_X_REF1X_UVA3X [mm]"); dxREF1UVA3->SetYTitle("Frequency"); dxREF1UVA3->SetLabelSize(0.045,"XY"); dxUVA3REF3->SetTitleSize(0.045,"XY");
TH1F* dxREF1REF3 = new TH1F(nameDxREF1REF3,"",400,-4,4); dxREF1REF3->SetXTitle("Delta_X_REF1X_REF3X [mm]"); dxREF1REF3->SetYTitle("Frequency"); dxREF1REF3->SetLabelSize(0.045,"XY"); dxREF1REF3->SetTitleSize(0.045,"XY");
TH1F* dyUVA3REF3 = new TH1F(nameDyUVA3REF3,"",400,-4,4); dyUVA3REF3->SetXTitle("Delta_Y_UVA3Y_REF3Y [mm]"); dyUVA3REF3->SetYTitle("Frequency"); dyUVA3REF3->SetLabelSize(0.045,"XY"); dyUVA3REF3->SetTitleSize(0.045,"XY");
TH1F* dyREF1UVA3 = new TH1F(nameDyREF1UVA3,"",400,-4,4); dyREF1UVA3->SetXTitle("Delta_Y_REF1Y_UVA3Y [mm]"); dyREF1UVA3->SetYTitle("Frequency"); dyREF1UVA3->SetLabelSize(0.045,"XY"); dyREF1UVA3->SetTitleSize(0.045,"XY");
TH1F* dyREF1REF3 = new TH1F(nameDyREF1REF3,"",400,-4,4); dyREF1REF3->SetXTitle("Delta_Y_REF1Y_REF3Y [mm]"); dyREF1REF3->SetYTitle("Frequency"); dyREF1REF3->SetLabelSize(0.045,"XY"); dyREF1REF3->SetTitleSize(0.045,"XY");
*/
TH1F* angleREF3 = new TH1F("angleREF3","Rotation angle distribution of REF3 and REF2",1000,-0.5,0.5); angleREF3->SetXTitle("Angle [radian]"); angleREF3->SetYTitle("Frequency");
//TH1F* angleREF3_2 = new TH1F("angleREF3","Get From Delta_y",2000,-0.5,3.5); angleREF3_2->SetXTitle("Rotation angle of ERF3 and REF2 [radian]"); angleREF3_2->SetYTitle("Frequency");
TH1F* angleUVA3 = new TH1F("angleUVA3","Rotation angle distribution of UVA3 and REF2",1000,-0.5,0.5); angleUVA3->SetXTitle("Angle [radian]"); angleUVA3->SetYTitle("Frequency");
//TH1F* angleUVA3_2 = new TH1F("angleUVA3_2","Get From Delta_y",2000,-0.5,3.5); angleUVA3_2->SetXTitle("Rotation angle of UVA3 and REF2 [radian]"); angleUVA3_2->SetYTitle("Frequency");
TH1F* angleREF1 = new TH1F("angleREF1","Rotation angle distribution of REF1 and REF2",1000,-0.5,0.5); angleREF1->SetXTitle("Angle [radian]"); angleREF1->SetYTitle("Frequency");
//TH1F* angleREF1_2 = new TH1F("angleREF1_2","Get From Delta_y",2000,-0.5,3.5); angleREF1_2->SetXTitle("Rotation angle of ERF1 and REF2 [radian]"); angleREF1_2->SetYTitle("Frequency");
//TH1F* angleEta5 = new TH1F("angleEta5","Rotation Eta5 and REF2",1000,-0.5,0.5);
//delta-y up: 200,-4,4
//TH1F* deltayZZ = new TH1F("deltaY","",200,-4,4); deltayZZ->SetXTitle("Position difference [mm]");deltayZZ->SetYTitle("Frequency");
//deltayZZ->SetLabelSize(0.045,"XY");deltayZZ->SetTitleSize(0.045,"XY");
//cout<<"shift: "<<shiREF2X<<"\t"<<shiREF2Y<<"\t"<<shiREF3X<<"\t"<<shiREF3Y<<"\t"<<shiUVA3X<<"\t"<<shiUVA3Y<<"\t"<<shiREF1X<<"\t"<<shiREF1Y<<endl;
TH1F* xTrackChi2 = new TH1F("XTrackChi2","Chi square of tracks in X projection",1000,0,0.2); xTrackChi2->SetXTitle("#chi^{2} of track in X"); xTrackChi2->SetYTitle("Frequency");
xTrackChi2->SetTitleSize(0.04,"XY"); xTrackChi2->SetLabelSize(0.04,"XY");
TH1F* yTrackChi2 = new TH1F("YTrackChi2","Chi square of tracks in Y projection",1000,0,0.2); yTrackChi2->SetXTitle("#chi^{2} of track in Y"); yTrackChi2->SetYTitle("Frequency");
yTrackChi2->SetTitleSize(0.04,"XY"); yTrackChi2->SetLabelSize(0.04,"XY");
fout<<"shift: "<<shiREF2X<<"\t"<<shiREF2Y<<"\t"<<shiREF3X<<"\t"<<shiREF3Y<<"\t"<<shiUVA3X<<"\t"<<shiUVA3Y<<"\t"<<shiREF1X<<"\t"<<shiREF1Y<<endl;
fout2<<"rotation: "<<aREF3REF2<<"\t"<<aUVA3REF2<<"\t"<<aREF1REF2<<endl;
int nnnn=0;
for(Int_t i=0;i<vpREF2X.size();i++){
//shift
vpREF2X[i] = vpREF2X[i] - shiREF2X; vpREF2Y[i] = vpREF2Y[i] - shiREF2Y;
vpREF3X[i] = vpREF3X[i] - shiREF3X; vpREF3Y[i] = vpREF3Y[i] - shiREF3Y;
vpUVA3X[i] = vpUVA3X[i] - shiUVA3X; vpUVA3Y[i] = vpUVA3Y[i] - shiUVA3Y;
vpREF1X[i] = vpREF1X[i] - shiREF1X; vpREF1Y[i] = vpREF1Y[i] - shiREF1Y;
// vpZZ1[i] = vpZZ1[i] - shiZZ1; vpZZ2[i] = vpZZ2[i] - shiZZ2;
//vpEta5[i] = vpEta5[i] - shiEta5;
tempREF2X=vpREF2X[i]; tempREF2Y=vpREF2Y[i]; tempREF3X=vpREF3X[i]; tempREF3Y=vpREF3Y[i];
tempUVA3X=vpUVA3X[i]; tempUVA3Y=vpUVA3Y[i]; tempREF1X=vpREF1X[i]; tempREF1Y=vpREF1Y[i];
//tempEta5=vpEta5[i];
//rotate back
vpREF3X[i]=tempREF3X*cos(aREF3REF2)-tempREF3Y*sin(aREF3REF2);
vpREF3Y[i]=tempREF3X*sin(aREF3REF2)+tempREF3Y*cos(aREF3REF2);
vpUVA3X[i]=tempUVA3X*cos(aUVA3REF2)-tempUVA3Y*sin(aUVA3REF2);
vpUVA3Y[i]=tempUVA3X*sin(aUVA3REF2)+tempUVA3Y*cos(aUVA3REF2);
vpREF1X[i]=tempREF1X*cos(aREF1REF2)-tempREF1Y*sin(aREF1REF2);
vpREF1Y[i]=tempREF1X*sin(aREF1REF2)+tempREF1Y*cos(aREF1REF2);
//vpEta5[i]=tempREF2X*sin(aEta5REF2)+tempEta5*cos(aEta5REF2);
hpREF2X->Fill(vpREF2X[i]); hpREF2Y->Fill(vpREF2Y[i]);
hpREF3X->Fill(vpREF3X[i]); hpREF3Y->Fill(vpREF3Y[i]);
hpUVA3X->Fill(vpUVA3X[i]); hpUVA3Y->Fill(vpUVA3Y[i]);
hpREF1X->Fill(vpREF1X[i]); hpREF1Y->Fill(vpREF1Y[i]);
//hpZZ1->Fill(vpZZ1[i]); hpZZ2->Fill(vpZZ2[i]);
//hpEta5->Fill(vpEta5[i]); //cout<<vpEta5[i]<<endl;
/*
dxREF3X->Fill(vpREF3X[i]-vpREF2X[i]); dyREF3Y->Fill(vpREF3Y[i]-vpREF2Y[i]);
dxUVA3X->Fill(vpUVA3X[i]-vpREF2X[i]); dyUVA3Y->Fill(vpUVA3Y[i]-vpREF2Y[i]);
dxREF1X->Fill(vpREF1X[i]-vpREF2X[i]); dyREF1Y->Fill(vpREF1Y[i]-vpREF2Y[i]);
dxUVA3REF3->Fill(vpUVA3X[i]-vpREF3X[i]); dyUVA3REF3->Fill(vpUVA3Y[i]-vpREF3Y[i]);
dxREF1UVA3->Fill(vpREF1X[i]-vpUVA3X[i]); dyREF1UVA3->Fill(vpREF1Y[i]-vpUVA3Y[i]);
dxREF1REF3->Fill(vpREF1X[i]-vpREF3X[i]); dyREF1REF3->Fill(vpREF1Y[i]-vpREF3Y[i]);
*/
TGraph* g1 = new TGraph();
g1->SetPoint(0,0, vpREF2X[i]);
g1->SetPoint(1,1143.5,vpREF3X[i]);
g1->SetPoint(2,2686.5,vpUVA3X[i]);
g1->SetPoint(3,3169.5,vpREF1X[i]);
TF1* f1 = new TF1("line1","[0]+[1]*x",0,3200);
g1->Fit("line1","Q");
double intercept1 = f1->GetParameter(0);
double slope1 = f1->GetParameter(1);
double MeasuredREF2X = intercept1 + slope1*0.0;
double MeasuredREF3X = intercept1 + slope1*1143.5;
double MeasuredUVA3X = intercept1 + slope1*2686.5;
double MeasuredREF1X = intercept1 + slope1*3169.5;
residualREF2X->Fill(MeasuredREF2X-vpREF2X[i]);
residualREF3X->Fill(MeasuredREF3X-vpREF3X[i]);
residualUVA3X->Fill(MeasuredUVA3X-vpUVA3X[i]);
residualREF1X->Fill(MeasuredREF1X-vpREF1X[i]);
xTrackChi2->Fill(f1->GetChisquare());
//cout<<f1->GetChisquare()<<"\t";
delete f1; delete g1;
TGraph* g2 = new TGraph();
g2->SetPoint(0,0, vpREF2Y[i]);
g2->SetPoint(1,1143.5,vpREF3Y[i]);
// exclusive
g2->SetPoint(2,2686.5,vpUVA3Y[i]);
g2->SetPoint(3,3169.5,vpREF1Y[i]);
// inclusive
//g2->SetPoint(2,2305.5,vpZZ2[i]); //inclusive 1
//g2->SetPoint(2,2327.5,vpZZ1[i]); //inclusive 2
//g2->SetPoint(3,2686.5,vpUVA3Y[i]);
//g2->SetPoint(4,3169.5,vpREF1Y[i]);
//g2->SetPoint(3,3169.5,vpREF1Y[i]);
TF1* f2 = new TF1("line2","[0]+[1]*x",0,3200);
g2->Fit("line2","Q");
double intercept2 = f2->GetParameter(0);
double slope2 = f2->GetParameter(1);
double MeasuredREF2Y = intercept2 + slope2*0.0;
double MeasuredREF3Y = intercept2 + slope2*1143.5;
double MeasuredUVA3Y = intercept2 + slope2*2686.5;
double MeasuredREF1Y = intercept2 + slope2*3169.5;
//double MeasuredZZ1 = intercept2 + slope2*2327.5;
//double MeasuredZZ2 = intercept2 + slope2*2305.5;
//double MeasuredEta5 = intercept2 + slope2*2011.5;
residualREF2Y->Fill(MeasuredREF2Y-vpREF2Y[i]);
residualREF3Y->Fill(MeasuredREF3Y-vpREF3Y[i]);
residualUVA3Y->Fill(MeasuredUVA3Y-vpUVA3Y[i]);
residualREF1Y->Fill(MeasuredREF1Y-vpREF1Y[i]);
//residualZZ1->Fill(MeasuredZZ1-vpZZ1[i]);
//residualZZ2->Fill(MeasuredZZ2-vpZZ2[i]);
//residualEta5->Fill(MeasuredEta5-vpEta5[i]);
//deltayZZ->Fill(vpZZ1[i]-vpZZ2[i]);
yTrackChi2->Fill(f2->GetChisquare());
//cout<<f2->GetChisquare()<<endl;
delete f2; delete g2;
double cosineREF3 = CalculateCosTheta1(vpREF2X[i],vpREF2Y[i],vpREF3X[i],vpREF3Y[i]);
double cosineUVA3 = CalculateCosTheta1(vpREF2X[i],vpREF2Y[i],vpUVA3X[i],vpREF3Y[i]);
double cosineREF1 = CalculateCosTheta1(vpREF2X[i],vpREF2Y[i],vpREF1X[i],vpREF3Y[i]);
// double cosineEta5 = CalculateCosTheta1(vpREF2X[i],vpREF2Y[i],vpREF2X[i],vpEta5[i]);
angleREF3->Fill(cosineREF3); //angleREF3_2->Fill(cosineREF3_2);
angleUVA3->Fill(cosineUVA3); //angleUVA3_2->Fill(cosineUVA3_2);
angleREF1->Fill(cosineREF1); //angleREF1_2->Fill(cosineREF1_2);
//angleEta5->Fill(cosineEta5);
nnnn++;
//if(nnnn%1000==0) cout<<nnnn<<"......"<<endl;
//if(nnnn>1000) break;
} //for loop
//cout<<"after for loop"<<endl;
gStyle->SetOptFit(1111);
I2GFvalues myValues;
myValues = I2GFmainLoop(residualREF2X, 1, 10 , 1);
meanREF2X = myValues.mean; sigmaREF2X=myValues.sigma;
myValues = I2GFmainLoop(residualREF2Y, 1, 10 , 1);
meanREF2Y = myValues.mean; sigmaREF2Y=myValues.sigma;
myValues = I2GFmainLoop(residualREF3X, 1, 10 , 1);
meanREF3X = myValues.mean; sigmaREF3X=myValues.sigma;
myValues = I2GFmainLoop(residualREF3Y, 1, 10 , 1);
meanREF3Y = myValues.mean; sigmaREF3Y=myValues.sigma;
myValues = I2GFmainLoop(residualUVA3X, 1, 10 , 1);
meanUVA3X = myValues.mean; sigmaUVA3X=myValues.sigma;
myValues = I2GFmainLoop(residualUVA3Y, 1, 10 , 1);
meanUVA3Y = myValues.mean; sigmaUVA3Y=myValues.sigma;
myValues = I2GFmainLoop(residualREF1X, 1, 10 , 1);
meanREF1X = myValues.mean; sigmaREF1X=myValues.sigma;
myValues = I2GFmainLoop(residualREF1Y, 1, 10 , 1);
meanREF1Y = myValues.mean; sigmaREF1Y=myValues.sigma;
cout<<"residual mean: "<<meanREF2X<<"\t"<<meanREF2Y<<"\t"<<meanREF3X<<"\t"<<meanREF3Y<<"\t"<<meanUVA3X<<"\t"<<meanUVA3Y<<"\t"<<meanREF1X<<"\t"<<meanREF1Y<<endl;
//<<meanZZ1<<"\t"<<meanZZ2<<endl;
fout1<<"residual mean: "<<meanREF2X<<"\t"<<meanREF2Y<<"\t"<<meanREF3X<<"\t"<<meanREF3Y<<"\t"<<meanUVA3X<<"\t"<<meanUVA3Y<<"\t"<<meanREF1X<<"\t"<<meanREF1Y<<endl;
//<<meanZZ1<<"\t"<<meanZZ2<<endl;
/*
myValues = I2GFmainLoop(dxREF3X, 1, 10 , 1);
myValues = I2GFmainLoop(dxUVA3X, 1, 10 , 1);
myValues = I2GFmainLoop(dxREF1X, 1, 10 , 1);
myValues = I2GFmainLoop(dyREF3Y, 1, 10 , 1);
myValues = I2GFmainLoop(dyUVA3Y, 1, 10 , 1);
myValues = I2GFmainLoop(dyREF1Y, 1, 10 , 1);
myValues = I2GFmainLoop(dxUVA3REF3, 1, 10 , 1);
myValues = I2GFmainLoop(dxREF1UVA3, 1, 10 , 1);
myValues = I2GFmainLoop(dxREF1REF3, 1, 10 , 1);
myValues = I2GFmainLoop(dyUVA3REF3, 1, 10 , 1);
myValues = I2GFmainLoop(dyREF1UVA3, 1, 10 , 1);
myValues = I2GFmainLoop(dyREF1REF3, 1, 10 , 1);
*/
myValues = I2GFmainLoop(angleREF3, 1, 10 , 1);
meanAngleREF3=myValues.mean;
myValues = I2GFmainLoop(angleUVA3, 1, 10 , 1);
meanAngleUVA3=myValues.mean;
myValues = I2GFmainLoop(angleREF1, 1, 10 , 1);
meanAngleREF1=myValues.mean;
//maximum=angleEta5->GetMean(); rms=angleEta5->GetRMS(1); lRange=maximum-rms*0.7; hRange=maximum+rms*0.7;
//TF1* funAngleEta5=new TF1("funAngleEta5","gaus",lRange,hRange); angleEta5->Fit("funAngleEta5","RQ");
//meanAngleEta5=funAngleEta5->GetParameter(1);
//maximum=xTrackChi2->GetMean(); rms=xTrackChi2->GetRMS(1); lRange=maximum-rms*0.5; hRange=maximum+rms*0.5;
//TF1* funXTrackChi2=new TF1("funXTrackChi2","gaus",lRange,hRange); xTrackChi2->Fit("funXTrackChi2","RQ");
meanXChi2=xTrackChi2->GetMean();
//maximum=yTrackChi2->GetMean(); rms=yTrackChi2->GetRMS(1); lRange=maximum-rms*0.5; hRange=maximum+rms*0.5;
//TF1* funYTrackChi2=new TF1("funYTrackChi2","gaus",lRange,hRange); yTrackChi2->Fit("funYTrackChi2","RQ");
meanYChi2=yTrackChi2->GetMean();
totalAngleREF3 += aREF3REF2;
totalAngleUVA3 += aUVA3REF2;
totalAngleREF1 += aREF1REF2;
fout3<<sigmaREF2X<<"\t"<<sigmaREF2Y<<"\t"<<sigmaREF3X<<"\t"<<sigmaREF3Y<<"\t"<<sigmaUVA3X<<"\t"<<sigmaUVA3Y<<"\t"<<sigmaREF1X<<"\t"<<sigmaREF1Y<<"\t"<<totalAngleREF3<<"\t"<<totalAngleUVA3<<"\t"<<totalAngleREF1<<"\t"<<meanXChi2<<"\t"<<meanYChi2<<endl;
f->Write();
f->Close();
//delete funPosEta5; delete funPosREF1Y; delete funPosUVA3Y; delete funPosREF3Y; delete funPosREF2Y;
double factor = -0.2;
//shiREF2X = meanREF2X*factor; shiREF2Y = meanREF2Y*factor;
//shiREF3X = meanREF3X*factor; shiREF3Y = meanREF3Y*factor;
//shiUVA3X = meanUVA3X*factor; shiUVA3Y = meanUVA3Y*factor;
//shiREF1X = meanREF1X*factor; shiREF1Y = meanREF1Y*factor;
//shiZZ1 = meanZZ1*factor; shiZZ2 = meanZZ2*factor;
//shiEta5 = meanEta5*factor;
factor=0.2;
shiREF2X=0.0; shiREF2Y=0.0;
shiREF3X=0.0; shiREF3Y=0.0;
shiUVA3X=0.0; shiUVA3Y=0.0;
shiREF1X=0.0; shiREF1Y=0.0;
aREF3REF2 = 0.001 ;
aUVA3REF2 = 0.0;
aREF1REF2 = 0.0;
//aUVA3REF2 = meanAngleUVA3*factor;
//aREF1REF2 = meanAngleREF1*factor;
// aEta5REF2 = meanAngleEta5*factor;
// if(iterNb>0) break; // only cylce once.
//if((meanREF2X>=-0.001 && meanREF2X<=0.001) && (meanREF2Y>=-0.001 && meanREF2Y<=0.001))
// if((meanREF3X>=-0.001 && meanREF3X<=0.001) && (meanREF3Y>=-0.001 && meanREF3Y<=0.001))
// if((meanUVA3X>=-0.001 && meanUVA3X<=0.001) && (meanUVA3Y>=-0.001 && meanUVA3Y<=0.001))
// if((meanREF1X>=-0.001 && meanREF1X<=0.001) && (meanREF1Y>=-0.001 && meanREF1Y<=0.001) && meanEta5<=0.005)
//if(meanAngleREF3>=-0.001 && meanAngleREF3<=0.001 && meanAngleUVA3>=-0.001 && meanAngleUVA3<=0.001 && meanAngleREF1>=-0.001 && meanAngleREF1<=0.001)
// {
// cout<<"find it...iterating "<<iterNb<<" times."<<endl;
// break;
// }
// break;
if(iterNb==41) break;
}//while(1)
fout.close();
fout1.close();
fout2.close();
} // entire script
int main(int argc, char* argv[]){
//std::vector<std::ifstream*> fin;
std::string finBaseName;
std::ofstream fout;
std::ofstream fout2("test_counts.csv");
//.. user input parameters
double intRange = 5.0;
int startFileId = 1;
int endFileId = 1;
int numFrame = 1;
float binWidth = 0.001f;
int maxNumBin = 0;
process_arg(finBaseName, fout, argc, argv, intRange, startFileId, endFileId);
//.. linked list parameters (for worms COM, not particles)
float dcell = float(intRange);
int nxcell, nycell, ncell;
const int ddx[5] = { 0, -1, 0, 1, 1 };
const int ddy[5] = { 0, 1, 1, 1, 0 };
int * ptz = { 0 };
int * heads = { 0 };
for (int fid = startFileId; fid <= endFileId; fid++)
{
//.. make ifstream and open
std::ostringstream ifname;
ifname << finBaseName << fid << ".xyz";
std::ifstream fin(ifname.str(), std::ios::in);
if (!fin.is_open()){
std::cerr << "Error opening file '" << ifname.str() << "'\n"
<< "Check for correct input name and/or directory\n" << std::endl;
continue;
}
// Simulation parameters
int numParticles;
int numPerWorm;
int numWorms;
double numWorms2;
char charTrash;
float k2spring;
float hx;
float hy;
float floatTrash;
float intRange2 = intRange * intRange;
float intArea = _PI * intRange2;
//.. loop through entire file
while (!fin.eof())
{
numParticles = 4; // Deals with case of black line at end of file.
fin >> numParticles;
fin >> numPerWorm >> k2spring >> hx >> hy;
numParticles -= 4;
if (numParticles == 0) break;
numWorms = numParticles / numPerWorm;
numWorms2 = numWorms*numWorms;
float hxo2 = hx / 2.0;
float hyo2 = hy / 2.0;
//.. make linked list
nxcell = int(ceil(hx / dcell));
nycell = int(ceil(hy / dcell));
ncell = nxcell*nycell;
// Allocate Containers
float* comx = new float[numWorms];
float* comy = new float[numWorms];
float* theta = new float[numWorms];
ptz = new int[numWorms];
heads = new int[ncell];
//.. init heads
for (int i = 0; i < ncell; i++)
heads[i] = -1;
// Read in X,Y,Z positions for frame
//.. and set linked list
std::cout << "Reading frame " << numFrame << " from " << ifname.str() << std::endl;
for (int w = 0; w < numWorms; w++)
{
//.. read in particles for worm w
float * x = new float[numPerWorm];
float * y = new float[numPerWorm];
for (int p = 0; p < numPerWorm; p++)
fin >> charTrash >> x[p] >> y[p] >> floatTrash;
//.. get COM for w
float x1 = x[0];
float y1 = y[0];
float x2 = x[numPerWorm - 1];
float y2 = y[numPerWorm - 1];
delete[] x;
delete[] y;
float dx = x1 - x2;
float dy = y1 - y2;
if (dx > hxo2){
dx -= hx;
x1 -= hx;
}
if (dx < -hxo2){
dx += hx;
x1 += hx;
}
if (dy > hyo2){
dy -= hy;
y1 -= hy;
}
if (dy < -hyo2){
dy += hy;
y1 += hy;
}
//.. calculate theta for worm w
theta[w] = std::atan2(dy, dx);
// Periodic Boundaries on COM
float Xcom = (x1 + x2) / 2.0;
float Ycom = (y1 + y2) / 2.0;
if (Xcom > hx) Xcom -= hx;
if (Xcom < 0) Xcom += hx;
if (Ycom > hy) Ycom -= hy;
if (Ycom < 0) Ycom += hy;
//.. add to linked list and store COM
int icell = int(floor(Xcom / dcell));
int jcell = int(floor(Ycom / dcell));
int scell = jcell*nxcell + icell;
ptz[w] = heads[scell];
heads[scell] = w;
comx[w] = Xcom;
comy[w] = Ycom;
}
// Dump corner particles at end of file to trash
for (int t = 0; t < 4; t++)
fin >> charTrash >> floatTrash >> floatTrash >> floatTrash;
// Components of calculation
float totalOrderParameter = 0;
float totalCos2Sum = 0;
float totalCosSinSum = 0;
float totalAveOver = 0;
//.. histogram for frame
std::vector<float> svd;
std::vector<int> numInBin;
//.. calculate using linked list
std::cout << "Calculating ... \n";
for (int ic = 0; ic < nxcell; ic++)
{
for (int jc = 0; jc < nycell; jc++)
{
//.. scalar add of cell
int scell = jc*nxcell + ic;
//std::cout << scell << "\t";
//.. look for stop flag
if (heads[scell] == -1) continue;
//.. loop over adjacent cells
for (int dir = 0; dir < 5; dir++)
{
//.. neighbor cell scalor address
int icnab = (ic + ddx[dir]) % nxcell;
int jcnab = (jc + ddy[dir]) % nycell;
if (icnab < 0) icnab += nxcell;
if (jcnab < 0) jcnab += nycell;
int scnab = jcnab * nxcell + icnab;
//.. loop for stop flag
if (heads[scnab] == -1) continue;
int iw = heads[scell];
while (iw >= 0)
{
// Components of calculation
float orderParameter = 0;
float cos2Sum = 0;
float cosSinSum = 0;
float aveOver = 0;
int jw = heads[scnab];
while (jw >= 0)
{
//std::cout << iw << ", " << jw << std::endl;
// Boundary conditions on Xcoms
float dXcom = comx[iw] - comx[jw];
float dYcom = comy[iw] - comy[jw];
if (dXcom > hxo2) dXcom -= hx;
if (dXcom < -hxo2) dXcom += hx;
if (dYcom > hyo2) dYcom -= hy;
if (dYcom < -hyo2) dYcom += hy;
//.. only if close enough
const float R2com = dXcom*dXcom + dYcom*dYcom;
if (R2com <= intRange2)
{
//.. relative angle between worms
float dtheta = theta[iw] - theta[jw];
//.. 2pi boundary conditons
if (dtheta > _PI) dtheta -= _2PI;
if (dtheta < -_PI) dtheta += _2PI;
cos2Sum += std::cos(dtheta)*std::cos(dtheta);
cosSinSum += std::cos(dtheta)*std::sin(dtheta);
totalCos2Sum += cos2Sum;
totalCosSinSum += cosSinSum;
totalAveOver += 1;
aveOver += 1;
}
jw = ptz[jw];
}
//.. add to histogram of S vs Density
// Form Averages and calculate OrderParameter for Frame
const float aveCos2 = cos2Sum / aveOver;
const float aveCosSin = cosSinSum / aveOver;
orderParameter = 2 * std::sqrt((aveCos2 - 0.5f)*(aveCos2 - 0.5f) + aveCosSin*aveCosSin);
//.. find bin and resize if necessary
int b = int((aveOver / intArea)/binWidth);
//std::cout << b << "," << svd.size() << "\t";
while (svd.size() <= b)
{
//std::cout << "in\t";
svd.push_back(0.0f);
numInBin.push_back(0);
}
//float stop;
//std::cout << b << "," << svd.size() << "\t";
//std::cin >> stop;
//.. add S to bin b
svd[b] += orderParameter;
numInBin[b]++;
//std::cin >> stop;
//.. move to next particle
iw = ptz[iw];
}
}
}
}
/*
//.. loop through all worms
for (int w1 = 0; w1 < numWorms; w1++)
{
//.. get head to tail vector
float x1 = x[w1][0];
float y1 = y[w1][0];
float x2 = x[w1][numPerWorm - 1];
float y2 = y[w1][numPerWorm - 1];
float dx = x1 - x2;
float dy = y1 - y2;
if (dx > hxo2){
dx -= hx;
x1 -= hx;
}
if (dx < -hxo2){
dx += hx;
x1 += hx;
}
if (dy > hyo2){
dy -= hy;
y1 -= hy;
}
if (dy < -hyo2){
dy += hy;
y1 += hy;
}
// Periodic Boundaries
float Xcom1 = (x1 + x2) / 2.0;
float Ycom1 = (y1 + y2) / 2.0;
if (Xcom1 > hx) Xcom1 -= hx;
if (Xcom1 < 0) Xcom1 += hx;
if (Ycom1 > hy) Ycom1 -= hy;
if (Ycom1 < 0) Ycom1 += hy;
//.. loop through all other unconsidered pairs
for (int w2 = w1 + 1; w2 < numWorms; w2++)
{
float _x1 = x[w2][0];
float _x2 = x[w2][numPerWorm - 1];
float _y1 = y[w2][0];
float _y2 = y[w2][numPerWorm - 1];
float _dx = _x1 - _x2;
float _dy = _y1 - _y2;
if (_dx > hxo2){
_dx -= hx;
_x1 -= hx;
}
if (_dx < -hxo2){
_dx += hx;
_x1 += hx;
}
if (_dy > hyo2){
_dy -= hy;
_y1 -= hy;
}
if (_dy < -hyo2){
_dy += hy;
_y1 += hy;
}
// Periodic Boundaries
float Xcom2 = (_x1 + _x2) / 2.0;
float Ycom2 = (_y1 + _y2) / 2.0;
if (Xcom2 > hx) Xcom2 -= hx;
if (Xcom2 < 0) Xcom2 += hx;
if (Ycom2 > hy) Ycom2 -= hy;
if (Ycom2 < 0) Ycom2 += hy;
// Boundary conditions on Xcoms
float dXcom = Xcom1 - Xcom2;
float dYcom = Ycom1 - Ycom2;
if (dXcom > hxo2) dXcom -= hx;
if (dXcom < -hxo2) dXcom += hx;
if (dYcom > hyo2) dYcom -= hy;
if (dYcom < -hyo2) dYcom += hy;
//.. check for close enough
const float R2com = dXcom*dXcom + dYcom*dYcom;
if (R2com > intRange2) continue;
//.. update properties
const float theta1 = std::atan2(dy, dx);
const float theta2 = std::atan2(_dy, _dx);
const float dtheta = theta1 - theta2;
cos2Sum += std::cos(dtheta)*std::cos(dtheta);
cosSinSum += std::cos(dtheta)*std::sin(dtheta);
aveOver += 1;
}
}*/
//.. print to file
const int numBin = svd.size();
fout2 << numFrame;
fout << numFrame++;
for (int i = 0; i < numBin; i++)
{
float printVal;
if (numInBin.at(i) > 0)
{
printVal = svd[i] / float(numInBin.at(i));
}
else
{
printVal = 0.0f;
}
fout << ", " << printVal;
fout2 << ", " << numInBin.at(i);
}
fout << std::endl;
fout2 << std::endl;
//.. store for printing bin labels
if (numBin > maxNumBin) maxNumBin = numBin;
//.. print to console
totalOrderParameter = 2 * std::sqrtf(std::powf((totalCos2Sum / totalAveOver - 0.5f), 2.0f) + std::powf(totalCosSinSum / totalAveOver, 2.0f));
std::cout << "S = " << totalOrderParameter << std::endl;
fout.flush();
//fout << numFrame++ << ", " << totalOrderParameter << std::endl;
// Handle Dynamic Arrays
delete[] comx;
delete[] comy;
delete[] theta;
delete[] ptz;
delete[] heads;
} // End of File
fin.close();
//totalOrderParameter /= float(numFrame);
//std::cout << "S_ave = " << totalOrderParameter << std::endl;
}
//.. print bin labels
fout << "bins";
for (int i = 1; i <= maxNumBin; i++)
fout << ", " << i*binWidth;
fout << std::endl;
fout.close();
fout2.close();
return EXIT_SUCCESS;
}
int main(int argc, char * argv[])
{
int width = 640;
int height = 480;
Resolution::getInstance(width, height);
Intrinsics::getInstance(528, 528, 320, 240);
cv::Mat intrinsicMatrix = cv::Mat(3,3,CV_64F);
intrinsicMatrix.at<double>(0,0) = Intrinsics::getInstance().fx();
intrinsicMatrix.at<double>(1,1) = Intrinsics::getInstance().fy();
intrinsicMatrix.at<double>(0,2) = Intrinsics::getInstance().cx();
intrinsicMatrix.at<double>(1,2) = Intrinsics::getInstance().cy();
intrinsicMatrix.at<double>(0,1) =0;
intrinsicMatrix.at<double>(1,0) =0;
intrinsicMatrix.at<double>(2,0) =0;
intrinsicMatrix.at<double>(2,1) =0;
intrinsicMatrix.at<double>(2,2) =1;
Bytef * decompressionBuffer = new Bytef[Resolution::getInstance().numPixels() * 2];
IplImage * deCompImage = 0;
std::string logFile="/home/lili/Kinect_Logs/2015-11-05.00.klg";
// assert(pcl::console::parse_argument(argc, argv, "-l", logFile) > 0 && "Please provide a log file");
RawLogReader logReader(decompressionBuffer,
deCompImage,
logFile,
true);
cv::Mat1b tmp(height, width);
cv::Mat3b depthImg(height, width);
PlaceRecognition placeRecognition(&intrinsicMatrix);
iSAMInterface iSAM;
//Keyframes
KeyframeMap map(true);
Eigen::Vector3f lastPlaceRecognitionTrans = Eigen::Vector3f::Zero();
Eigen::Matrix3f lastPlaceRecognitionRot = Eigen::Matrix3f::Identity();
int64_t lastTime = 0;
OdometryProvider * odom = 0;
//int frame_index = 0;
// uint64_t timestamp;
/*if(true)
{
odom = new FOVISOdometry;
if(logReader.hasMore())
{
logReader.getNext();
Eigen::Matrix3f Rcurr = Eigen::Matrix3f::Identity();
Eigen::Vector3f tcurr = Eigen::Vector3f::Zero();
odom->getIncrementalTransformation(tcurr,
Rcurr,
logReader.timestamp,
(unsigned char *)logReader.deCompImage->imageData,
(unsigned short *)&decompressionBuffer[0]);
}
}*/
//else
// {
odom = new DVOdometry;
if(logReader.hasMore())
{
logReader.getNext();
DVOdometry * dvo = static_cast<DVOdometry *>(odom);
dvo->firstRun((unsigned char *)logReader.deCompImage->imageData,
(unsigned short *)&decompressionBuffer[0]);
}
//}
ofstream fout1("camera_pose_DVOMarch28.txt");
ofstream fout2("camera_pose_KeyframeMotionMetric0.1March28.txt");
ofstream fout3("loop_closure_transformationMarch28.txt");
ofstream fout4("camera_pose_after_optimizationMarch28.txt");
ofstream fout5("camera_pose_after_optimizationMarch28DVOCov.txt");
ofstream fout6("camera_pose_after_optimizationMarch28DVOLoopTransCov.txt");
/*
pcl::visualization::PCLVisualizer cloudViewer;
cloudViewer.setBackgroundColor(1, 1, 1);
cloudViewer.initCameraParameters();
cloudViewer.addCoordinateSystem(0.1, 0, 0, 0);
*/
//pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> color(cloud->makeShared());
//cloudViewer.addPointCloud<pcl::PointXYZRGB>(cloud->makeShared(), color, "Cloud Viewer");
int loopClosureCount=0;
while(logReader.hasMore())
{
logReader.getNext();
cv::Mat3b rgbImg(height, width, (cv::Vec<unsigned char, 3> *)logReader.deCompImage->imageData);
cv::Mat1w depth(height, width, (unsigned short *)&decompressionBuffer[0]);
cv::normalize(depth, tmp, 0, 255, cv::NORM_MINMAX, 0);
cv::cvtColor(tmp, depthImg, CV_GRAY2RGB);
cv::imshow("RGB", rgbImg);
cv::imshow("Depth", depthImg);
char key = cv::waitKey(1);
if(key == 'q')
{
break;
}
else if(key == ' ')
{
key = cv::waitKey(0);
}
if(key == 'q')
{
break;
}
Eigen::Matrix3f Rcurr = Eigen::Matrix3f::Identity();
Eigen::Vector3f tcurr = Eigen::Vector3f::Zero();
// #1
odom->getIncrementalTransformation(tcurr,
Rcurr,
logReader.timestamp,
(unsigned char *)logReader.deCompImage->imageData,
(unsigned short *)&decompressionBuffer[0]);
fout1<<tcurr[0]<<" "<<tcurr[1]<<" "<<tcurr[2]<<" "<<Rcurr(0,0)<<" "<<Rcurr(0,1)<<" "<<Rcurr(0,2)<<" "<<Rcurr(1,0)<<" "<<Rcurr(1,1)<<" "<<Rcurr(1,2)<<" "<<Rcurr(2,0)<<" "<<Rcurr(2,1)<<" "<<Rcurr(2,2)<<endl;
Eigen::Matrix3f Rdelta = Rcurr.inverse() * lastPlaceRecognitionRot;
Eigen::Vector3f tdelta = tcurr - lastPlaceRecognitionTrans;
//Eigen::MatrixXd covariance = odom->getCovariance();
//Eigen::MatrixXd covariance=Eigen::Matrix<double, 6, 6>::Identity()* 1e-3;
if((Projection::rodrigues2(Rdelta).norm() + tdelta.norm()) >= 0.1)
{
Eigen::MatrixXd covariance = odom->getCovariance();
iSAM.addCameraCameraConstraint(lastTime,
logReader.timestamp,
lastPlaceRecognitionRot,
lastPlaceRecognitionTrans,
Rcurr,
tcurr);
//covariance);
printCovariance(fout5, covariance);
lastTime = logReader.timestamp;
lastPlaceRecognitionRot = Rcurr;
lastPlaceRecognitionTrans = tcurr;
cout<<"before add keyframe"<<endl;
// #2
map.addKeyframe((unsigned char *)logReader.deCompImage->imageData,
(unsigned short *)&decompressionBuffer[0],
Rcurr,
tcurr,
logReader.timestamp);
fout2<<tcurr[0]<<" "<<tcurr[1]<<" "<<tcurr[2]<<" "<<Rcurr(0,0)<<" "<<Rcurr(0,1)<<" "<<Rcurr(0,2)<<" "<<Rcurr(1,0)<<" "<<Rcurr(1,1)<<" "<<Rcurr(1,2)<<" "<<Rcurr(2,0)<<" "<<Rcurr(2,1)<<" "<<Rcurr(2,2)<<endl;
/*
//Save keyframe
{
cv::Mat3b rgbImgKeyframe(height, width, (cv::Vec<unsigned char, 3> *)logReader.deCompImage->imageData);
cv::Mat1w depthImgKeyframe(height, width, (unsigned short *)&decompressionBuffer[0]);
//save keyframe depth
char fileName[1024] = {NULL};
sprintf(fileName, "keyframe_depth_%06d.png", frame_index);
cv::imwrite(fileName, depthImgKeyframe);
//save keyframe rgb
sprintf(fileName, "keyframe_rgb_%06d.png", frame_index);
cv::imwrite(fileName, rgbImgKeyframe);
frame_index ++;
}
*/
int64_t matchTime;
Eigen::Matrix4d transformation;
// Eigen::MatrixXd cov(6,6);
//isam::Covariance(0.001 * Eigen::Matrix<double, 6, 6>::Identity()))
Eigen::MatrixXd cov=0.001 * Eigen::Matrix<double, 6, 6>::Identity();
cout<<"map.addKeyframe is OK"<<endl;
// #3
if(placeRecognition.detectLoop((unsigned char *)logReader.deCompImage->imageData,
(unsigned short *)&decompressionBuffer[0],
logReader.timestamp,
matchTime,
transformation,
cov,
loopClosureCount))
{
//printCovariance(fout6, cov);
cout<<"logReader.timestamp "<<logReader.timestamp<<endl;
cout<<"matchTime "<<matchTime<<endl;
/*
transformation << -0.2913457145219732, 0.228056050293173, -0.9290361201559172, 2.799184934345601,
0.6790194052589797, 0.7333821627861707, -0.03291277242681545, 1.310438143604587,
0.673832562222562, -0.6404225489719699, -0.3685222338703895, 6.988973505496276,
0, 0, 0, 0.999999999999998;
*/
/*
transformation << 0.9998996846969838, 0.003948215234314986, -0.01360265192291004, 0.05847011404293689,
-0.004032877285312574, 0.9999726343121815, -0.006202138950136233, 0.04528938486109094,
0.01357779229749574, 0.006256374606648019, 0.9998882444218992, 0.02203456132723125,
0, 0, 0, 1;
*/
iSAM.addLoopConstraint(logReader.timestamp, matchTime, transformation);//, cov);
fout3<<transformation(0,0)<<" "<<transformation(0,1)<<" "<<transformation(0,2)<<" "<<transformation(0,3)<<" "<<transformation(1,0)<<" "<<transformation(1,1)<<" "<<transformation(1,2)<<" "<<transformation(1,3)<<" "<<transformation(2,0)<<" "<<transformation(2,1)<<" "<<transformation(2,2)<<" "<<transformation(2,3)<<" "<<transformation(3,0)<<" "<<transformation(3,1)<<" "<<transformation(3,2)<<" "<<transformation(3,3)<<endl;
loopClosureCount++;
}
}
if(loopClosureCount>=1)
{
break;
}
}
/*
for(int i=0; i<loopClosureCount;i++)
{
iSAM.addLoopConstraint(placeRecognition.loopClosureConstraints.at(i)->time1,
placeRecognition.loopClosureConstraints.at(i)->time2,
placeRecognition.loopClosureConstraints.at(i)->constraint);
}*/
std::vector<std::pair<uint64_t, Eigen::Matrix4f> > posesBefore;
iSAM.getCameraPoses(posesBefore);
cout<<"It works good before optimization"<<endl;
// #4
double residual =iSAM.optimise();
cout<<"It works good after optimize and before map.applyPoses"<<endl;
// map.applyPoses(isam);
//cout<<"It works good before *cloud=map.getMap and after map.applyPoses(isam)"<<endl;
/*
pcl::PointCloud<pcl::PointXYZRGB> *cloud = map.getMap();
// Write it back to disk under a different name.
// Another possibility would be "savePCDFileBinary()".
cout<<"before storing the point cloud map"<<endl;
pcl::io::savePCDFileASCII ("outputCloudMap03DVODensity005.pcd", *cloud);
cout << "Saved data points to outputMap.pcd." << std::endl;
cout<<"copy data into octomap..."<<endl;
octomap::ColorOcTree tree( 0.05 );
for (size_t i=0; i<(*cloud).points.size(); i++)
{
// 将点云里的点插入到octomap中
tree.updateNode( octomap::point3d((*cloud).points[i].x, (*cloud).points[i].y, (*cloud).points[i].z), true );
}
for (size_t i=0; i<(*cloud).points.size(); i++)
{
tree.integrateNodeColor( (*cloud).points[i].x, (*cloud).points[i].y, (*cloud).points[i].z, (*cloud).points[i].r, (*cloud).points[i].g, (*cloud).points[i].b);
}
tree.updateInnerOccupancy();
tree.write("OctomapColorLab03DVODensity005.ot");
cout<<"please see the done."<<endl;
*/
//pcl::visualization::PCLVisualizer cloudViewer;
// cloudViewer.setBackgroundColor(1, 1, 1);
//cloudViewer.initCameraParameters();
// cloudViewer.addCoordinateSystem(0.1, 0, 0, 0);
//pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> color(cloud->makeShared());
//cloudViewer.addPointCloud<pcl::PointXYZRGB>(cloud->makeShared(), color, "Cloud Viewer");
std::vector<std::pair<uint64_t, Eigen::Matrix4f> > newPoseGraph;
iSAM.getCameraPoses(newPoseGraph);
/*
for(unsigned int i = 0; i < newPoseGraph.size(); i++)
{
// file << std::setprecision(6) << std::fixed << (double)newPoseGraph.at(i).first / 1000000.0 << " ";
Eigen::Vector3f trans = newPoseGraph.at(i).second.topRightCorner(3, 1);
Eigen::Matrix3f rot = newPoseGraph.at(i).second.topLeftCorner(3, 3);
fout4 << trans(0) << " " << trans(1) << " " << trans(2) << " ";
Eigen::Quaternionf currentCameraRotation(rot);
//file << currentCameraRotation.x() << " " << currentCameraRotation.y() << " " << currentCameraRotation.z() << " " << currentCameraRotation.w() << "\n";
}*/
for(std::vector<std::pair<uint64_t, Eigen::Matrix4f> >::iterator ite=newPoseGraph.begin(); ite!=newPoseGraph.end(); ite++)
{
Eigen::Matrix3f Roptimized;
Roptimized<<ite->second(0,0), ite->second(0,1), ite->second(0,2),
ite->second(1,0), ite->second(1,1), ite->second(1,2),
ite->second(2,0), ite->second(2,1), ite->second(2,2);
Eigen::Quaternionf quatOptimized(Roptimized);
fout4<<ite->second(0,3)<<" "<<ite->second(1,3)<<" "<<ite->second(2,3)<<" "<<quatOptimized.w()<<" "<<quatOptimized.x()<<" "<<quatOptimized.y()<<" "<<quatOptimized.z()<<endl;
}
cout<<"The number of optimized poses"<<newPoseGraph.size()<<endl;
// drawPoses(poses, cloudViewer, 1.0, 0, 0);
//drawPoses(posesBefore, cloudViewer, 0, 0, 1.0);
//cloudViewer.spin();
delete [] decompressionBuffer;
return 0;
}
