2 #include "trg/cdc/Fitter3DUtility.h"
3 #include "trg/cdc/Hough3DUtility.h"
4 #include "trg/cdc/JSignal.h"
5 #include "trg/cdc/JLUT.h"
6 #include "trg/cdc/JSignalData.h"
7 #include "trg/cdc/FpgaUtility.h"
18 m_mode(0), m_nWires(), m_rr(), m_ztostraw(), m_anglest(),
19 m_cotStart(0), m_cotEnd(0), m_z0Start(0), m_z0End(0),
20 m_nCotSteps(0), m_nZ0Steps(0), m_cotStepSize(0), m_z0StepSize(0),
21 m_houghMeshLayerDiff(0), m_houghMeshLayer(0), m_houghMesh(0), m_houghMeshDiff(0),
22 m_hitMap(0), m_driftMap(0), m_geoCandidatesIndex(0), m_geoCandidatesPhi(0),
23 m_geoCandidatesDiffStWires(0), m_stAxPhi(), m_bestCot(0), m_bestZ0(0),
24 m_houghMax(0), m_minDiffHough(0), m_foundZ(), m_foundPhiSt(), m_bestTSIndex(),
25 m_bestTS(), m_inputFileName(
"GeoFinder.input"), m_findRhoMax(0), m_findRhoMin(0),
26 m_findRhoIntMax(0), m_findRhoIntMin(0),
27 m_findPhi0Max(0), m_findPhi0Min(0), m_findPhi0IntMax(0), m_findPhi0IntMin(0),
28 m_findArcCosMax(0), m_findArcCosMin(0), m_findArcCosIntMax(0), m_findArcCosIntMin(0),
29 m_findPhiZMax(0), m_findPhiZMin(0), m_findPhiZIntMax(0), m_findPhiZIntMin(0),
30 m_rhoMax(0), m_rhoMin(0), m_rhoBit(0), m_phi0Max(0), m_phi0Min(0), m_phi0Bit(0),
31 m_stAxWireFactor(0), m_LUT(0),
32 m_arcCosLUT(0), m_wireConvertLUT(0),
33 m_commonData(0), m_outputVhdlDirname(
"./VHDL/finder3D")
40 for (
int iSt = 0; iSt < 4; iSt++) {
42 for (
int iTS = 0; iTS <
m_nWires[iSt] / 2; iTS++) {
83 for (
int iLayer = 0; iLayer < 4; iLayer++) {
87 for (
int i = 0; i < 4; i++) {
88 m_rr[i] = geometryVariables[i];
91 m_nWires[i] = (int)geometryVariables[i + 12];
106 cout <<
"[Error] 3DFinder mode is not correct. Current mode is " <<
m_mode <<
"." << endl;
128 void Hough3DFinder::runFinder(std::vector<double>& trackVariables, vector<vector<double> >& stTSs, vector<vector<int> >& stTSDrift)
132 for (
int iLayer = 0; iLayer < 4; iLayer++) {
138 int charge = (int)trackVariables[0];
139 double rho = trackVariables[1];
140 double fitPhi0 = trackVariables[2];
143 vector<double > tsArcS;
144 vector<vector<double> > tsZ;
145 for (
unsigned i = 0; i < 4; i++) {
147 tsZ.push_back(vector<double>());
148 for (
unsigned j = 0; j < stTSs[i].size(); j++) {
179 m_z0End = (int)initVariables[3];
206 if (
false) cout << initVariables.size() << endl;
213 for (
int iLayer = 0; iLayer < 4; iLayer++)
m_geoCandidatesPhi->push_back(vector<double> ());
221 if (
false) cout << initVariables.size() << endl;
225 for (
int i = 0; i < 4; i++) {
227 for (
int j = 0; j <
m_nWires[i] / 2; j++) {
233 for (
int iSt = 0; iSt < 4; iSt++) {
235 for (
int iTS = 0; iTS <
m_nWires[iSt] / 2; iTS++) {
245 for (
int iLayer = 0; iLayer < 4; iLayer++)
m_geoCandidatesPhi->push_back(vector<double> ());
296 for (
int i = 0; i < 4; i++) {
300 for (
int iSt = 0; iSt < 4; iSt++) {
322 vector<vector<double> >& tsZ)
325 int charge = (int)trackVariables[0];
326 double rho = trackVariables[1];
327 double fitPhi0 = trackVariables[2];
332 for (
int k = 0; k < 4; k++) {
342 double tempZ0Start, tempZ0End;
343 double tempZ01, tempZ02;
345 double actualCot, actualZ0;
350 for (
int cotStep = 0; cotStep <
m_nCotSteps; cotStep++) {
357 for (
unsigned iLayer = 0; iLayer < 4; iLayer++) {
358 for (
unsigned iTS = 0; iTS < stTSs[iLayer].size(); iTS++) {
360 tempZ01 = -tsArcS[iLayer] * tempCotStart + tsZ[iLayer][iTS];
361 tempZ02 = -tsArcS[iLayer] * tempCotEnd + tsZ[iLayer][iTS];
364 if (tempZ01 < tempZ02) {
365 tempZ0Start = tempZ01;
368 tempZ0Start = tempZ02;
375 if (tempZ0Start > 0) {
395 for (
int z0Step =
int(tempZ0Start); z0Step <= int(tempZ0End); z0Step++) {
399 cout <<
"cutoff because z0step is bigger or smaller than z0 limit ";
406 }
else { tempHoughZ0 = z0Step; }
416 m_minDiffHough = abs(actualCot * tsArcS[iLayer] + actualZ0 - tsZ[iLayer][iTS]);
427 for (
int houghCot = 0; houghCot <
m_nCotSteps; houghCot++) {
428 for (
int houghZ0 = 0; houghZ0 <
m_nZ0Steps; houghZ0++) {
433 tempHoughZ0 = houghZ0;
439 if (
false) cout << actualCot << actualZ0 << endl;
441 for (
int layer = 0; layer < 4; layer++) {
452 for (
int houghCot = 0; houghCot <
m_nCotSteps; houghCot++) {
453 for (
int houghZ0 = 0; houghZ0 <
m_nZ0Steps; houghZ0++) {
457 }
else { tempHoughZ0 = houghZ0;}
492 for (
int i = 0; i < 4; i++) {
502 double minDiff[4] = {999, 999, 999, 999};
503 for (
unsigned iLayer = 0; iLayer < 4; iLayer++) {
504 for (
unsigned iTS = 0; iTS < stTSs[iLayer].size(); iTS++) {
505 if (minDiff[iLayer] > abs(
m_foundPhiSt[iLayer] - stTSs[iLayer][iTS])) {
506 minDiff[iLayer] = abs(
m_foundPhiSt[iLayer] - stTSs[iLayer][iTS]);
507 m_bestTS[iLayer] = stTSs[iLayer][iTS];
517 std::vector<std::vector<int> >& stTSDrift)
521 for (
int iLayer = 0; iLayer < 4; iLayer++) {
522 (*m_geoCandidatesIndex)[iLayer].clear();
523 (*m_geoCandidatesPhi)[iLayer].clear();
524 (*m_geoCandidatesDiffStWires)[iLayer].clear();
527 int charge = (int)trackVariables[0];
528 double rho = trackVariables[1];
529 double fitPhi0 = trackVariables[2];
533 for (
int iLayer = 0; iLayer < 4; iLayer++) {
537 for (
unsigned iTS = 0; iTS < stTSs[iLayer].size(); iTS++) {
540 int t_priorityPosition = (stTSDrift[iLayer][iTS] & 3);
544 if (t_priorityPosition != 3)
continue;
546 tsDiffSt =
m_stAxPhi[iLayer] - stTSs[iLayer][iTS];
552 if (iLayer % 2 == 0) {
553 if (tsDiffSt > 0 && tsDiffSt <= 10) {
554 (*m_geoCandidatesIndex)[iLayer].push_back(iTS);
555 (*m_geoCandidatesPhi)[iLayer].push_back(stTSs[iLayer][iTS]);
556 (*m_geoCandidatesDiffStWires)[iLayer].push_back(tsDiffSt);
559 if (tsDiffSt < 0 && tsDiffSt >= -10) {
560 (*m_geoCandidatesIndex)[iLayer].push_back(iTS);
561 (*m_geoCandidatesPhi)[iLayer].push_back(stTSs[iLayer][iTS]);
562 (*m_geoCandidatesDiffStWires)[iLayer].push_back(tsDiffSt);
583 double meanWireDiff[4] = { 3.68186, 3.3542, 3.9099, 4.48263 };
584 for (
int iLayer = 0; iLayer < 4; iLayer++) {
590 double bestDiff = 999;
592 tsDiffSt =
m_stAxPhi[iLayer] - stTSs[iLayer][(*m_geoCandidatesIndex)[iLayer][iTS]];
598 if (abs(abs(tsDiffSt) - meanWireDiff[iLayer]) < bestDiff) {
600 bestDiff = abs(abs(tsDiffSt) - meanWireDiff[iLayer]);
601 m_bestTS[iLayer] = stTSs[iLayer][(*m_geoCandidatesIndex)[iLayer][iTS]];
602 m_bestTSIndex[iLayer] = (*m_geoCandidatesIndex)[iLayer][iTS];
618 vector<vector<int> >& stTSDrift)
621 int m_verboseFlag =
m_mBool[
"fVerbose"];
623 if (m_verboseFlag) cout <<
"####geoFinder start####" << endl;
626 for (
int iLayer = 0; iLayer < 4; iLayer++) {
627 (*m_geoCandidatesPhi)[iLayer].clear();
628 (*m_geoCandidatesIndex)[iLayer].clear();
629 (*m_geoCandidatesDiffStWires)[iLayer].clear();
632 for (
int iLayer = 0; iLayer < 4; iLayer++) {
633 for (
int iTS = 0; iTS <
m_nWires[iLayer] / 2; iTS++) {
638 for (
int iSt = 0; iSt < 4; iSt++) {
639 for (
int iTS = 0; iTS <
m_nWires[iSt] / 2; iTS++) {
645 int charge = (int)trackVariables[0];
646 double rho = trackVariables[1];
647 double fitPhi0 = trackVariables[2];
652 for (
unsigned iLayer = 0; iLayer < 4; iLayer++) {
654 for (
unsigned iTS = 0; iTS < stTSs[iLayer].size(); iTS++) {
655 iHitTS = int(stTSs[iLayer][iTS] *
m_nWires[iLayer] / 2 / 2 /
m_Trg_PI + 0.5);
656 driftInfo = stTSDrift[iLayer][iTS];
657 if (m_verboseFlag) cout <<
"[" << iLayer <<
"] TSId: " << iHitTS <<
" stTSs: " << stTSs[iLayer][iTS] <<
" driftInfo:" << driftInfo
658 <<
" priorityPosition:" << (driftInfo & 3) << endl;
700 std::map<std::string, std::vector<double> > mConstV;
701 std::map<std::string, double > mConstD;
702 std::map<std::string, double > mDouble;
714 double rhoMax = 2500;
718 mConstV[
"rr"] = vector<double> (9);
719 mConstV[
"rr3D"] = vector<double> (4);
720 mConstV[
"nTSs"] = vector<double> (9);
721 for (
unsigned iSt = 0; iSt < 4; iSt++) {
723 mConstV[
"rr"][2 * iSt + 1] =
m_rr[iSt] * 100;
724 mConstV[
"rr3D"][iSt] =
m_rr[iSt] * 100;
725 mConstV[
"nTSs"][2 * iSt + 1] =
m_nWires[iSt] / 2;
727 mConstD[
"acosLUTInBitSize"] = rhoBitSize;
728 mConstD[
"acosLUTOutBitSize"] = phiBitSize - 1;
733 mDouble[
"rho"] = rho * 100;
734 if (mDouble[
"rho"] > rhoMax) {
735 mDouble[
"rho"] = rhoMax;
736 mDouble[
"pt"] = rhoMax * 0.3 * 1.5 * 0.01;
741 mDouble[
"phi0"] = fitPhi0;
744 if (mDouble[
"phi0"] > mConstD[
"Trg_PI"]) mDouble[
"phi0"] -= 2 * mConstD[
"Trg_PI"];
745 else if (mDouble[
"phi0"] < -mConstD[
"Trg_PI"]) mDouble[
"phi0"] += 2 * mConstD[
"Trg_PI"];
752 vector<tuple<string, double, int, double, double, int> > t_values = {
753 make_tuple(
"phi0", mDouble[
"phi0"], phiBitSize, phiMin, phiMax, 0),
754 make_tuple(
"rho", mDouble[
"rho"], rhoBitSize, rhoMin, rhoMax, 0),
755 make_tuple(
"charge", (
int)(charge == 1 ? 1 : 0), 1, 0, 1.5, 0),
768 for (
unsigned iSt = 0; iSt < 4; iSt++) {
769 string t_invMinName =
"invPhiAxMin_" + to_string(iSt);
770 double t_actual =
m_mSignalStorage[
"rho_c_" + to_string(iSt)].getMinActual();
773 string t_invMaxName =
"invPhiAxMax_" + to_string(iSt);
781 for (
unsigned iSt = 0; iSt < 4; iSt++) {
782 string t_valueName =
"phiAx_" + to_string(iSt);
783 string t_minName =
"phiAxMin_" + to_string(iSt);
784 string t_maxName =
"phiAxMax_" + to_string(iSt);
785 string t_invMinName =
"invPhiAxMin_" + to_string(iSt);
786 string t_invMaxName =
"invPhiAxMax_" + to_string(iSt);
790 double t_parameter = mConstV.at(
"rr3D")[iSt];
792 [ = ](
double aValue) ->
double{
return acos(t_parameter / 2 / aValue);},
795 (int)mConstD.at(
"acosLUTInBitSize"), (int)mConstD.at(
"acosLUTOutBitSize"));
801 for (
unsigned iSt = 0; iSt < 4; iSt++) {
803 string t_valueName =
"phiAx_" + to_string(iSt);
810 vector<pair<Belle2::TRGCDCJSignal, vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > > > t_data;
815 vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > t_assigns = {
822 t_data.push_back(make_pair(t_compare, t_assigns));
833 t_data.push_back(make_pair(t_compare, t_assigns));
856 for (
unsigned iSt = 0; iSt < 4; iSt++) {
857 string t_valueName =
"dPhiAx_" + to_string(iSt);
859 string t_maxName =
"dPhiAxMax_" + to_string(iSt);
860 string t_minName =
"dPhiAxMin_" + to_string(iSt);
861 string t_2PiName =
"dPhiAx2Pi_" + to_string(iSt);
869 for (
unsigned iSt = 0; iSt < 4; iSt++) {
870 string t_in1Name =
"dPhiAx_" + to_string(iSt);
872 string t_valueName =
"dPhiAx_c_" + to_string(iSt);
873 string t_maxName =
"dPhiAxMax_" + to_string(iSt);
874 string t_minName =
"dPhiAxMin_" + to_string(iSt);
875 string t_2PiName =
"dPhiAx2Pi_" + to_string(iSt);
877 vector<pair<Belle2::TRGCDCJSignal, vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > > > t_data;
881 vector<pair<Belle2::TRGCDCJSignal*, Belle2::TRGCDCJSignal> > t_assigns = {
885 t_data.push_back(make_pair(t_compare, t_assigns));
893 t_data.push_back(make_pair(t_compare, t_assigns));
901 t_data.push_back(make_pair(t_compare, t_assigns));
912 for (
unsigned iSt = 0; iSt < 4; iSt++) {
913 int nShiftBits = int(log(pow(2, 24) * 2 * mConstD.at(
"Trg_PI") / mConstV.at(
"nTSs")[2 * iSt + 1] /
916 t_name =
"wireFactor_" + to_string(iSt);
924 for (
unsigned iSt = 0; iSt < 4; iSt++) {
926 iSt)].getToReal()) / log(2);
928 iSt)] *
m_mSignalStorage[
"wireFactor_" + to_string(iSt)]).shift(nShiftBits, 0);
932 vector< int > nCandidates = { 10, 10, 10, 13 };
934 vector<vector<bool> > t_stCandHitmap(4);
935 for (
unsigned iSt = 0; iSt < 4; iSt++) t_stCandHitmap[iSt] = vector<bool> (nCandidates[iSt]);
937 vector<vector<int> > t_stCandDriftmap(4);
938 for (
unsigned iSt = 0; iSt < 4; iSt++) t_stCandDriftmap[iSt] = vector<int> (nCandidates[iSt] + 1);
940 vector<tuple<string, double, int, double, double, int> > resultValues;
942 vector<pair<string, int> > t_chooseSignals = {
943 make_pair(
"wireDPhiAx_0", 1), make_pair(
"wireDPhiAx_1", 1), make_pair(
"wireDPhiAx_2", 1), make_pair(
"wireDPhiAx_3", 1)
947 vector<double> t_wireDPhiAx = {std::get<1>(resultValues[0]), std::get<1>(resultValues[1]), std::get<1>(resultValues[2]), std::get<1>(resultValues[3])};
950 for (
int iSt = 0; iSt < 4; iSt++) {
951 int indexTS = t_wireDPhiAx[iSt];
952 for (
int iTS = 0; iTS < nCandidates[iSt]; iTS++) {
954 t_stCandHitmap[iSt][iTS] =
m_hitMap[iSt][indexTS];
955 t_stCandDriftmap[iSt][iTS] =
m_driftMap[iSt][indexTS];
960 if (indexTS < 0) indexTS =
m_nWires[iSt] / 2 - 1;
964 if (indexTS >=
m_nWires[iSt] / 2) indexTS = 0;
970 for (
unsigned iLayer = 0; iLayer < 4; iLayer++) {
971 cout <<
"iSt:" << iLayer <<
" t_wireDPhiAx:" << t_wireDPhiAx[iLayer];
973 if (iLayer % 2 == 0) t_endTSId = t_wireDPhiAx[iLayer] - (nCandidates[iLayer] - 1);
974 else t_endTSId = t_wireDPhiAx[iLayer] + (nCandidates[iLayer] - 1);
975 if (t_endTSId < 0) t_endTSId += mConstV[
"nTSs"][2 * iLayer + 1];
976 else if (t_endTSId >= mConstV[
"nTSs"][2 * iLayer + 1]) t_endTSId -= mConstV[
"nTSs"][2 * iLayer + 1];
978 cout <<
" t_stCandHitmap[" << iLayer <<
"]: " << t_endTSId <<
"=> ";
979 for (
int iTS = nCandidates[iLayer] - 1; iTS >= 0; iTS--) {
980 cout << t_stCandHitmap[iLayer][iTS];
982 cout <<
" <= " << t_wireDPhiAx[iLayer] << endl;
991 vector<double> t_targetWirePosition = { 3, 2, 3, 3};
993 vector<int> t_bestRelTSId(4);
994 for (
unsigned iSt = 0; iSt < 4; iSt++) t_bestRelTSId[iSt] = nCandidates[iSt];
995 vector<int> t_bestDiff = {16, 16, 16, 16};
997 for (
unsigned iSt = 0; iSt < 4; iSt++) {
998 for (
int iTS = 0; iTS < nCandidates[iSt]; iTS++) {
999 int t_priorityPosition = (t_stCandDriftmap[iSt][iTS] & 3);
1001 if (t_stCandHitmap[iSt][iTS] == 0)
continue;
1002 if (t_priorityPosition != 3)
continue;
1003 double tsDiffTarget = fabs(t_targetWirePosition[iSt] - iTS);
1004 if (t_bestDiff[iSt] > tsDiffTarget) {
1005 t_bestDiff[iSt] = tsDiffTarget;
1006 t_bestRelTSId[iSt] = iTS;
1011 if (m_verboseFlag) {
1012 for (
unsigned iSt = 0; iSt < 4;
1013 iSt++) cout <<
"iSt:" << iSt <<
" bestRelTS:" << t_bestRelTSId[iSt] <<
" diff:" << t_bestDiff[iSt] << endl;
1016 vector<double> t_bestTSId(4);
1017 for (
unsigned iSt = 0; iSt < 4; iSt++) {
1018 if (iSt % 2 == 0) t_bestTSId[iSt] = t_wireDPhiAx[iSt] - t_bestRelTSId[iSt];
1019 else t_bestTSId[iSt] = t_wireDPhiAx[iSt] + t_bestRelTSId[iSt];
1022 vector<double> t_bestDriftInfo(4);
1023 for (
unsigned iSt = 0; iSt < 4; iSt++) t_bestDriftInfo[iSt] = t_stCandDriftmap[iSt][t_bestRelTSId[iSt]];
1027 if (m_verboseFlag) {
1028 for (
unsigned iSt = 0; iSt < 4; iSt++)cout <<
"iSt:" << iSt <<
" bestDriftInfo:" << t_bestDriftInfo[iSt] << endl;
1032 vector<double> t_bestTSId_c(4);
1033 for (
unsigned iSt = 0; iSt < 4; iSt++) {
1034 if (t_bestTSId[iSt] >= mConstV[
"nTSs"][2 * iSt + 1]) t_bestTSId_c[iSt] = t_bestTSId[iSt] - mConstV[
"nTSs"][2 * iSt + 1];
1035 else if (t_bestTSId[iSt] < 0) t_bestTSId_c[iSt] = t_bestTSId[iSt] + mConstV[
"nTSs"][2 * iSt + 1];
1036 else t_bestTSId_c[iSt] = t_bestTSId[iSt];
1039 if (m_verboseFlag) {
1040 for (
unsigned iSt = 0; iSt < 4; iSt++)cout <<
"iSt:" << iSt <<
" bestTS_c:" << t_bestTSId_c[iSt] << endl;
1044 for (
unsigned iSt = 0; iSt < 4; iSt++) {
1045 if (t_bestDriftInfo[iSt] != 0)
m_bestTS[iSt] = t_bestTSId_c[iSt] * 2 * mConstD[
"Trg_PI"] / mConstV[
"nTSs"][2 * iSt + 1] ;
1052 for (
unsigned iSt = 0; iSt < 4; iSt++) {
1053 if (t_bestDriftInfo[iSt] == 0) {
1056 for (
unsigned iTS = 0; iTS < stTSs[iSt].size(); iTS++) {
1057 if (fabs(
m_bestTS[iSt] - stTSs[iSt][iTS]) < 0.0001) {
1072 (*it).second.setName((*it).first);
1121 for (
unsigned iSt = 0; iSt < 4;
1128 if (m_verboseFlag) cout <<
"####geoFinder end####" << endl;
1137 if (input ==
"bestCot") {
1140 if (input ==
"bestZ0") {
1143 if (input ==
"houghMax") {
1146 if (input ==
"minDiffHough") {
1149 if (input ==
"foundZ") {
1155 if (input ==
"foundPhiSt") {
1161 if (input ==
"bestTS") {
1167 if (input ==
"bestTSIndex") {
1173 if (input ==
"st0GeoCandidatesPhi") {
1177 if (input ==
"st1GeoCandidatesPhi") {
1181 if (input ==
"st2GeoCandidatesPhi") {
1185 if (input ==
"st3GeoCandidatesPhi") {
1190 if (input ==
"st0GeoCandidatesDiffStWires") {
1194 if (input ==
"st1GeoCandidatesDiffStWires") {
1198 if (input ==
"st2GeoCandidatesDiffStWires") {
1202 if (input ==
"st3GeoCandidatesDiffStWires") {
1207 if (input ==
"st0GeoCandidatesIndex") {
1211 if (input ==
"st1GeoCandidatesIndex") {
1215 if (input ==
"st2GeoCandidatesIndex") {
1219 if (input ==
"st3GeoCandidatesIndex") {
1224 if (input ==
"stAxPhi") {
1231 if (input ==
"extreme") {
1254 if (input ==
"hitmapLayer1") {
1255 for (
unsigned iOutput = 0; iOutput < unsigned(
m_nWires[0] / 2); iOutput++) {
1256 result.push_back(
m_hitMap[0][iOutput]);
1260 if (input ==
"hitmapLayer2") {
1261 for (
unsigned iOutput = 0; iOutput < unsigned(
m_nWires[1] / 2); iOutput++) {
1262 result.push_back(
m_hitMap[1][iOutput]);
1266 if (input ==
"hitmapLayer3") {
1267 for (
unsigned iOutput = 0; iOutput < unsigned(
m_nWires[2] / 2); iOutput++) {
1268 result.push_back(
m_hitMap[2][iOutput]);
1272 if (input ==
"hitmapLayer4") {
1273 for (
unsigned iOutput = 0; iOutput < unsigned(
m_nWires[3] / 2); iOutput++) {
1274 result.push_back(
m_hitMap[3][iOutput]);