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; }