void scan3d::triangulate_stereo(const cv::Mat & K1, const cv::Mat & kc1, const cv::Mat & K2, const cv::Mat & kc2, const cv::Mat & Rt, const cv::Mat & T, const cv::Point2d & p1, const cv::Point2d & p2, cv::Point3d & p3d, double * distance) { //to image camera coordinates cv::Mat inp1(1, 1, CV_64FC2), inp2(1, 1, CV_64FC2); inp1.at<cv::Vec2d>(0, 0) = cv::Vec2d(p1.x, p1.y); inp2.at<cv::Vec2d>(0, 0) = cv::Vec2d(p2.x, p2.y); cv::Mat outp1, outp2; cv::undistortPoints(inp1, outp1, K1, kc1); cv::undistortPoints(inp2, outp2, K2, kc2); assert(outp1.type()==CV_64FC2 && outp1.rows==1 && outp1.cols==1); assert(outp2.type()==CV_64FC2 && outp2.rows==1 && outp2.cols==1); const cv::Vec2d & outvec1 = outp1.at<cv::Vec2d>(0,0); const cv::Vec2d & outvec2 = outp2.at<cv::Vec2d>(0,0); cv::Point3d u1(outvec1[0], outvec1[1], 1.0); cv::Point3d u2(outvec2[0], outvec2[1], 1.0); //to world coordinates cv::Point3d w1 = u1; cv::Point3d w2 = cv::Point3d(cv::Mat(Rt*(cv::Mat(u2) - T))); //world rays cv::Point3d v1 = w1; cv::Point3d v2 = cv::Point3d(cv::Mat(Rt*cv::Mat(u2))); //compute ray-ray approximate intersection p3d = approximate_ray_intersection(v1, w1, v2, w2, distance); }
/* ******************************************** * reweighting and pmf calculation at room temperature * ********************************************/ int main(int argc, char *argv[]){ // INPUT_PARAMETERS // argv[1]: input parameter file if( argv[1]==NULL ){ puts("No ARGUMEMTS"); puts("USAGE: ./a.out (argv[1]: input parameter file)" ); return 1; } cout<<"Your input-parameter file: "<<argv[1]<<endl; Inp_nishi inp1( argv[1] ); // ############# READ INPUT ######################################## /* (1) * */ cout<<endl<<"------- (1) load --------- \n"; string infile = inp1.read("INFILE"); string inprob = inp1.read("INPROB"); cout<<"# output"<<endl; string outpmf = inp1.read("OUTPMF"); cout<<"# settings"<<endl; int frame = atoi( inp1.read("NUMSTRUCTURE").c_str() ); /* DEFINITION OF VARIABLES */ vector<long double> prob2; //probability of each data vector<long double> ene_prob, prob; //from inprob (Pc) Probability distribution function along potential energy int outlier = 0; //num of data out of energy range /* OPEN FILE */ //float tmp; long double tmp; ifstream ifs1(infile.c_str()); //ifstream ifs1(infile.c_str(),ifstream::in); if(ifs1.fail()){ cerr<<"cannot open file "<<infile<<endl; //exit(1); return 1; } vector<float> c1, c2, pote; //while(!ifs.eof()){ //while(ifs.good()){ for(int ii=0;ii<frame;ii++){ ifs1 >> tmp; c1.push_back(tmp); ifs1 >> tmp; c2.push_back(tmp); ifs1 >> tmp; pote.push_back(tmp); } ifs1.close(); //cout<<"DEBUG: pote.size() = "<<pote.size()<<endl; //cout<<"DEBUG: pote[pote.size() -1] = "<<pote[pote.size() -1]<<endl; /* (2) assignment of probability * * */ // read file ifstream test(inprob.c_str()); //ifstream test(inprob.c_str(),ifstream::in); if(inprob == "NO") goto flag_noprob; //assign probability { // goto flag_noprob if(test.fail()){ cerr<<"cannot open file "<<inprob<<endl; //exit(1); return 1; } //test.clear(); //test.seekg(2); test >> tmp; //why outside of while-loop? because of ifs.eof //cerr<<"DEBUG: tmp of ene_prob = "<<tmp<<endl; //for(int ii=0;ii<2131;ii++){ //while( true ){ while( ! test.eof() ){ //while(test.good()){ //test >> tmp; //if( test.eof() ) break; //cerr<<"DEBUG: tmp of ene_prob = "<<tmp<<endl; ene_prob.push_back(tmp); test >> tmp; prob.push_back(tmp); test >> tmp; } test.close(); //cout<<"DEBUG: ene_prob.size() = "<<ene_prob.size()<<endl; cout<<" reading input section end "<<endl; //cout<<"DEBUG: prob.size() = "<<prob.size()<<endl; //cout<<"DEBUG: prob[prob.size() -1] = "<<prob[prob.size() -1]<<endl; //cout<<"DEBUG: ene_prob.size() = "<<ene_prob.size()<<endl; //cout<<"DEBUG: ene_prob[ene_prob.size() -1] = "<<ene_prob[prob.size() -1]<<endl; //ttp_v_mcmd input string inttpin = inp1.read("INTTPIN"); //ttp_v_mcmd.inp string inttpout = inp1.read("INTTPOUT"); //ttp_v_mcmd.out int interval = atoi( inp1.read("INTERVAL").c_str() ); vector<float> tmpvec; ifstream ttpvinp(inttpin.c_str()); //ttp_v_mcmd.inp if( ttpvinp.fail() ){ cerr<<"cannot open file "<<inttpin<<endl; return 1; } while( ttpvinp ){ string tmpstr; ttpvinp >> tmpstr; //cout<< tmpstr<<endl; if( tmpstr[0] == ';' ){ ttpvinp.ignore(100, '\n'); } else{ tmpvec.push_back( atof( tmpstr.c_str() ) ); } } ttpvinp.close(); /*for(unsigned int i;i<tmpvec.size();i++){ cout<<tmpvec[i]<<endl; }*/ int vst = tmpvec[0]; int trans = tmpvec[1]; vector<float> ttplimit; for(int i=1;i<=vst;i++){ ttplimit.push_back( tmpvec[4*i-2] ); ttplimit.push_back( tmpvec[4*i-1] ); } /*for(unsigned int i;i<ttplimit.size();i++){ cout<<ttplimit[i]<<endl; }*/ tmpvec.clear(); vector<int> currvst; ifstream ttpvout(inttpout.c_str()); //ttp_v_mcmd.out if( ttpvout.fail() ){ cerr<<"cannot open file "<<inttpout<<endl; return 1; } while( ttpvout ){ ttpvout >> tmp; ttpvout >> tmp; //cout<<tmp<<endl; currvst.push_back( tmp ); } /*for(unsigned int i;i<currvst.size();i++){ cout<<currvst[i]<<endl; }*/ ttpvout.close(); // cout<<endl<<"------- (2) assignment of probability --------- \n"; int check_flat[ene_prob.size()]; for(unsigned int i=0;i<ene_prob.size();i++){ check_flat[i] = 0; //initializing by zero } //trans/interval int iii=0; for(int ii=0;ii<frame;ii++){ int flag_1 = 0; unsigned int jj = 0; int vstnum = currvst[int(ii/20)]; //cout<<int(ii/20)<<" "<<vstnum<<endl; if( pote[ii] < ttplimit[2*vstnum-2] || pote[ii]>=ttplimit[2*vstnum-1] ){ prob2.push_back( 0 ); outlier ++ ; continue; } else if( pote[ii] < ene_prob[0] || pote[ii] >= ene_prob[ene_prob.size() - 1]){ prob2.push_back( 0 ); outlier ++ ; continue; } else{ for(jj=0;jj<ene_prob.size();jj++){ if( pote[ii] < ene_prob[jj] ){ prob2.push_back( prob[jj] ); if( vstnum == 1 || vstnum == vst ){ check_flat[jj] ++ ; } check_flat[jj] ++ ; flag_1 = 1; break; } } } //if( jj==ene_prob.size() ){ //if( pote[ii] >= ene_prob[ene_prob.size() - 1]){ //if( flag_1 == 0 ){ //cout<<"WARNING: pote["<<ii<<"] = "<<pote[ii]<<endl; //prob2.push_back( prob[ jj - 1 ] ); //check_flat[jj - 1 ] ++ ; //} } if( prob2.size() != frame ){ cout<<"ERROR: assignment of probability failed \n"; cout<<"num of assigned = "<<prob2.size()<<endl; cout<<"num of structures = "<<frame<<endl; return 1; } else{ cout<<"assignment of probability was ended successfully"<<endl; cout<<"num of outliers = "<<outlier<<endl; } string outcheck = inp1.read("OUTCHECK").c_str() ; if( outcheck != "NO" ){ ofstream ofs2; //check_flat //ofs2.open( inp1.read("OUTCHECK").c_str() ); ofs2.open( outcheck.c_str() ); for(unsigned int i = 0;i<ene_prob.size();i++){ ofs2<<ene_prob[i]<<" "<<check_flat[i]<<"\n"; } ofs2.close(); cout<<endl<<outcheck<<" was output successfully"<<endl; } } // end of goto flag_noprob flag_noprob: if(inprob == "NO"){ for(int ii=0;ii<frame;ii++){ prob2.push_back(1); } } /* OUTPUT FILE */ ofstream ofs1; ofs1.open( inp1.read("OUTPROB").c_str() ); for(unsigned int i = 0;i<prob2.size();i++){ ofs1<<prob2[i]<<"\n"; } ofs1.close(); /* (7) PMF calculation * * */ cout<<endl<<"REPORT> (7) PMF calculation \n"; float dcbound1 = atof( inp1.read("DCBOUND1").c_str() ); float dcbound2 = atof( inp1.read("DCBOUND2").c_str() ); //string pmfcalculation = inp1.read("PMFCALCULATION"); string pmfcalculation = "YES"; if( pmfcalculation == "YES" ){ double length_bin = atof(inp1.read("BINSIZE").c_str()); float emin = atof(inp1.read("MINCOORD").c_str()); float temperature = atof(inp1.read("TEMPERATURE").c_str()); int num_bin = atoi(inp1.read("NUMBIN").c_str()); cout<<"Range: "<<emin<<" to "<<emin+length_bin*num_bin<<endl; cout<<"Num of Partitions = NUMBIN*NUMBIN = "<<num_bin*num_bin<<endl; //float pmf[num_bin][num_bin]; double pmf[num_bin][num_bin]; //long double pmf[num_bin][num_bin]; for(int i=0;i<num_bin;i++){ //initialize array for(int j=0;j<num_bin;j++){ pmf[j][i] = 0; } } int count_pmf = 0; long double normcons = 0; int count_valid = 0; for(unsigned int n=0;n<frame;n++){ //count int flag_2 = 0; for(int i=0;i<num_bin;i++){ bool flag_3 = false; for(int j=0;j<num_bin;j++){ if(c1[n] >= emin + length_bin * j && c1[n] < emin + length_bin * (j + 1) && c2[n] >= emin + length_bin * i && c2[n] < emin + length_bin * (i + 1) ){ //pmf[j][i] ++ ; if( prob2[n] > 0 ){ count_valid ++ ; } if( pote[n] < dcbound1 || pote[n] > dcbound2 ){ pmf[j][i] = pmf[j][i] + prob2[n]*2 ; //counting by probability normcons = normcons + prob2[n]*2; //normalization constant } else{ pmf[j][i] = pmf[j][i] + prob2[n] ; //counting by probability normcons = normcons + prob2[n]; //normalization constant } count_pmf ++ ; //counting 1 by 1 //goto NEXT_PMF; flag_3 = true; flag_2 = 1; break; } } if( flag_3 ) break; } if( flag_2 == 0 ){ cout<<"WARNING: structure "<<n<<" was not assigned into any bin of pmf\n"; cout<<"c1[n] = "<<c1[n]<<", c2[n] = "<<c2[n]<<endl; } } cout<<"Normaliztion constant = "<<normcons<<endl; cout<<"The number of data whose probability is larger than zero = "<<count_valid<<endl; cout<<"count_pmf / frame = "<<count_pmf<<" / "<<frame<<endl; if( count_pmf != (int)frame )cout<<"WARNING: count_pmf != frame; "<<count_pmf<<" != "<<frame<<endl; double min_pmf = 999999, max_pmf = -999999; for(int i=0;i<num_bin;i++){ //normalization for(int j=0;j<num_bin;j++){ //pmf[j][i] = pmf[j][i] / frame; pmf[j][i] = pmf[j][i] / normcons; if( pmf[j][i] <= min_pmf ){ min_pmf = pmf[j][i]; } if( pmf[j][i] >= max_pmf ){ max_pmf = pmf[j][i]; } } } cout<<"Maximum Probability = "<<max_pmf<<endl; cout<<"Minimum Probability = "<<min_pmf<<endl; //double const_boltz = 1.380658e-23, pmf_temp = 300; min_pmf = 999999, max_pmf = -999999; int min_pmf_n[2]; double min_pmf_c[2]; min_pmf_n[0] = 0; min_pmf_n[1] = 0; int max_pmf_n[2]; double max_pmf_c[2]; max_pmf_n[0] = 0; max_pmf_n[1] = 0; for(int i=0;i<num_bin;i++){ //PMF calculation for(int j=0;j<num_bin;j++){ if( pmf[j][i] == 0 ){ //pmf[j][i] = 100; continue; } pmf[j][i] = -1 * BOLTZMAN_CONST * temperature * log( pmf[j][i] ); if( pmf[j][i] < min_pmf ){ min_pmf = pmf[j][i]; min_pmf_n[0] = j; min_pmf_n[1] = i; } if( pmf[j][i] > max_pmf ){ max_pmf = pmf[j][i]; max_pmf_n[0] = j; max_pmf_n[1] = i; //cout<<"DEBUG> max_pmf = "<<max_pmf<<", max_pmf_n[0], max_pmf_n[1] = "<<max_pmf_n[0]<<", "<<max_pmf_n[1]<<endl; } } } /*min_pmf_c[0] = emin + min_pmf_n[0] * length_bin + length_bin / 2; min_pmf_c[1] = emin + min_pmf_n[1] * length_bin + length_bin / 2; max_pmf_c[0] = emin + max_pmf_n[0] * length_bin + length_bin / 2; max_pmf_c[1] = emin + max_pmf_n[1] * length_bin + length_bin / 2;*/ min_pmf_c[0] = emin + min_pmf_n[0] * length_bin ; min_pmf_c[1] = emin + min_pmf_n[1] * length_bin ; max_pmf_c[0] = emin + max_pmf_n[0] * length_bin ; max_pmf_c[1] = emin + max_pmf_n[1] * length_bin ; cout<<"Maximum PMF = "<<max_pmf-min_pmf<<" (J/mol) = "<<(max_pmf-min_pmf)/JOULE_CALORIE/1000<<" (kcal/mol)\n"; cout<<"Minimum PMF = "<<min_pmf-min_pmf<<" (J/mol) = "<<(min_pmf-min_pmf)/JOULE_CALORIE/1000<<" (kcal/mol)\n"; cout<<"(c1, c2) of Maximum PMF = ("<<max_pmf_c[0]<<", "<<max_pmf_c[1]<<")\n"; cout<<"(c1, c2) of Minimum PMF = ("<<min_pmf_c[0]<<", "<<min_pmf_c[1]<<")\n"; FILE *fout; string outpmf = inp1.read("OUTPMF"); if((fout = fopen( outpmf.c_str(),"w" )) == NULL ){ printf("cannot open output file: %s\n", outpmf.c_str() ); return 1; } //for R format /*for(int i=0;i<num_bin;i++){ fprintf(fout,"%12.3f ", emin+ length_bin * i); } fprintf(fout,"\n"); for(int i=0;i<num_bin;i++){ fprintf(fout,"%12.3f ",emin+ length_bin * i); for(int j=0;j<num_bin;j++){ if( pmf[j][i] == 0L ){ fprintf(fout,"%12.3f ", 1000.0 ); } else{ fprintf(fout,"%12.3f ", (pmf[j][i] - min_pmf) / JOULE_CALORIE /1000L ); } } fprintf(fout,"\n"); }*/ //for gnuplot format for(int i=0;i<num_bin;i++){ for(int j=0;j<num_bin;j++){ if( pmf[j][i] == 0L ){ fprintf(fout,"%12.3f%12.3f%12.3f \n", emin + length_bin * j , emin + length_bin * i , 1000.0 ); } else{ fprintf(fout,"%12.3f%12.3f%12.3lf \n", emin + length_bin * j , emin + length_bin * i , (pmf[j][i] - min_pmf) / JOULE_CALORIE /1000L ); } } fprintf(fout,"\n"); } fclose( fout ); cout<<"output "<<outpmf<<endl; //end: /* structure list */ cout<<"\n\nWrite structure-number of minimum PMF bin \n"; for(unsigned int n=0;n<frame;n++){ //count /*if(c1[n] > min_pmf_c[0] - length_bin / 2 && c1[n] <= min_pmf_c[0] + length_bin / 2 && c2[n] > min_pmf_c[1] - length_bin / 2 && c2[n] <= min_pmf_c[1] + length_bin / 2 ){*/ if(c1[n] > min_pmf_c[0] && c1[n] <= min_pmf_c[0] + length_bin && c2[n] > min_pmf_c[1] && c2[n] <= min_pmf_c[1] + length_bin && prob2[n] > 0 ){ cout<<n+1<<" "<<pote[n]<<" "<<prob2[n]<<endl; } } // END } cout<<"\n\nit took "<<(float)clock()/CLOCKS_PER_SEC<<" sec of CPU to execute this program"<<endl; return 0; return 0; }
int main() { { static_assert( ranges::detail::BidirectionalCursor< ranges::detail::reverse_cursor<bidirectional_iterator<const char *>>>{}, ""); static_assert( ranges::detail::BidirectionalCursor< ranges::detail::reverse_cursor<random_access_iterator<const char *>>>{}, ""); static_assert( ranges::detail::RandomAccessCursor< ranges::detail::reverse_cursor<random_access_iterator<const char *>>>{}, ""); static_assert( ranges::BidirectionalIterator< ranges::reverse_iterator<bidirectional_iterator<const char *>>>{}, ""); static_assert( ranges::RandomAccessIterator< ranges::reverse_iterator<random_access_iterator<const char *>>>{}, ""); } { // test test<bidirectional_iterator<const char *>>(); test<random_access_iterator<char *>>(); test<char *>(); test<const char *>(); } { // test 2 const char s[] = "123"; test2(bidirectional_iterator<const char *>(s)); test2(random_access_iterator<const char *>(s)); } { // test3 Derived d; test3<bidirectional_iterator<Base *>>( bidirectional_iterator<Derived *>(&d)); test3<random_access_iterator<const Base *>>( random_access_iterator<Derived *>(&d)); } { // test4 const char *s = "1234567890"; random_access_iterator<const char *> b(s); random_access_iterator<const char *> e(s + 10); while (b != e) test4(b++); } { // test5 const char *s = "1234567890"; test5(bidirectional_iterator<const char *>(s), bidirectional_iterator<const char *>(s), false); test5(bidirectional_iterator<const char *>(s), bidirectional_iterator<const char *>(s + 1), true); test5(random_access_iterator<const char *>(s), random_access_iterator<const char *>(s), false); test5(random_access_iterator<const char *>(s), random_access_iterator<const char *>(s + 1), true); test5(s, s, false); test5(s, s + 1, true); } { const char *s = "123"; test6(bidirectional_iterator<const char *>(s + 1), bidirectional_iterator<const char *>(s)); test6(random_access_iterator<const char *>(s + 1), random_access_iterator<const char *>(s)); test6(s + 1, s); } { const char *s = "123"; test7(bidirectional_iterator<const char *>(s + 1), bidirectional_iterator<const char *>(s)); test7(random_access_iterator<const char *>(s + 1), random_access_iterator<const char *>(s)); test7(s + 1, s); } { const char *s = "1234567890"; test8(random_access_iterator<const char *>(s + 5), 5, random_access_iterator<const char *>(s)); test8(s + 5, 5, s); } { const char *s = "1234567890"; test9(random_access_iterator<const char *>(s + 5), 5, random_access_iterator<const char *>(s)); test9(s + 5, 5, s); } { const char *s = "123"; test10(bidirectional_iterator<const char *>(s + 1), bidirectional_iterator<const char *>(s + 2)); test10(random_access_iterator<const char *>(s + 1), random_access_iterator<const char *>(s + 2)); test10(s + 1, s + 2); } { const char *s = "123"; test11(bidirectional_iterator<const char *>(s + 1), bidirectional_iterator<const char *>(s + 2)); test11(random_access_iterator<const char *>(s + 1), random_access_iterator<const char *>(s + 2)); test11(s + 1, s + 2); } { const char *s = "1234567890"; test12(random_access_iterator<const char *>(s + 5), 5, random_access_iterator<const char *>(s + 10)); test12(s + 5, 5, s + 10); } { const char *s = "1234567890"; test13(random_access_iterator<const char *>(s + 5), 5, random_access_iterator<const char *>(s + 10)); test13(s + 5, 5, s + 10); } { A a; test14(&a + 1, A()); } { Derived d; test15<bidirectional_iterator<Base *>>( bidirectional_iterator<Derived *>(&d)); test15<random_access_iterator<const Base *>>( random_access_iterator<Derived *>(&d)); test15<Base *>(&d); } { const char *s = "1234567890"; test16(bidirectional_iterator<const char *>(s), bidirectional_iterator<const char *>(s), true); test16(bidirectional_iterator<const char *>(s), bidirectional_iterator<const char *>(s + 1), false); test16(random_access_iterator<const char *>(s), random_access_iterator<const char *>(s), true); test16(random_access_iterator<const char *>(s), random_access_iterator<const char *>(s + 1), false); test16(s, s, true); test16(s, s + 1, false); } { char s[3] = {0}; test17(random_access_iterator<const char *>(s), random_access_iterator<char *>(s), 0); random_access_iterator<char *> inp1(s); test17(random_access_iterator<char *>(s), random_access_iterator<const char *>(s + 1), 1); test17(random_access_iterator<const char *>(s + 1), random_access_iterator<char *>(s), -1); test17(s, s, 0); test17(s, s + 1, 1); test17(s + 1, s, -1); } { const char *s = "1234567890"; test18(random_access_iterator<const char *>(s), random_access_iterator<const char *>(s), false); test18(random_access_iterator<const char *>(s), random_access_iterator<const char *>(s + 1), true); test18(random_access_iterator<const char *>(s + 1), random_access_iterator<const char *>(s), false); test18(s, s, false); test18(s, s + 1, true); test18(s + 1, s, false); } { const char *s = "1234567890"; test19(random_access_iterator<const char *>(s), random_access_iterator<const char *>(s), true); test19(random_access_iterator<const char *>(s), random_access_iterator<const char *>(s + 1), true); test19(random_access_iterator<const char *>(s + 1), random_access_iterator<const char *>(s), false); test19(s, s, true); test19(s, s + 1, true); test19(s + 1, s, false); } { const char *s = "1234567890"; test20(random_access_iterator<const char *>(s + 5), 4, '1'); test20(s + 5, 4, '1'); } { const char *s = "1234567890"; test21(random_access_iterator<const char *>(s), random_access_iterator<const char *>(s), false); test21(random_access_iterator<const char *>(s), random_access_iterator<const char *>(s + 1), false); test21(random_access_iterator<const char *>(s + 1), random_access_iterator<const char *>(s), true); test21(s, s, false); test21(s, s + 1, false); test21(s + 1, s, true); } { const char* s = "1234567890"; test22(random_access_iterator<const char*>(s), random_access_iterator<const char*>(s), false); test22(random_access_iterator<const char*>(s), random_access_iterator<const char*>(s+1), false); test22(random_access_iterator<const char*>(s+1), random_access_iterator<const char*>(s), true); test22(s, s, false); test22(s, s+1, false); test22(s+1, s, true); } { const char* s = "1234567890"; test23(random_access_iterator<const char*>(s), random_access_iterator<const char*>(s), true); test23(random_access_iterator<const char*>(s), random_access_iterator<const char*>(s+1), false); test23(random_access_iterator<const char*>(s+1), random_access_iterator<const char*>(s), true); test23(s, s, true); test23(s, s+1, false); test23(s+1, s, true); } { B a; test24(&a+1, B()); } { C l[3] = {C(0), C(1), C(2)}; auto ri = ranges::rbegin(l); CHECK ( (*ri).get() == 2 ); ++ri; CHECK ( (*ri).get() == 1 ); ++ri; CHECK ( (*ri).get() == 0 ); ++ri; CHECK ( ri == ranges::rend(l)); } { const char* s = "1234567890"; test25(random_access_iterator<const char*>(s+5), 5, random_access_iterator<const char*>(s)); test25(s+5, 5, s); } return test_result(); }