 |
GeneralBrokenLines
V01-11-00
|
00001 /* 00002 * GblTrajectory.cpp 00003 * 00004 * Created on: Aug 18, 2011 00005 * Author: kleinwrt 00006 */ 00007 00081 #include "GblTrajectory.h" 00082 00084 00091 GblTrajectory::GblTrajectory(bool flagCurv, bool flagU1dir, bool flagU2dir) : 00092 numPoints(0), numOffsets(0), numCurvature(flagCurv ? 1 : 0), numParameters( 00093 0), numLocals(0), externalPoint(0), theDimension(0), thePoints(), theData(), externalIndex(), externalSeed() { 00094 // TODO Auto-generated constructor stub 00095 if (flagU1dir) 00096 theDimension.push_back(0); 00097 if (flagU2dir) 00098 theDimension.push_back(1); 00099 thePoints.reserve(100); // reserve some space for points 00100 } 00101 00102 GblTrajectory::~GblTrajectory() { 00103 // TODO Auto-generated destructor stub 00104 } 00105 00107 00111 unsigned int GblTrajectory::addPoint(GblPoint aPoint) { 00112 numPoints++; 00113 aPoint.setLabel(numPoints); 00114 thePoints.push_back(aPoint); 00115 numLocals = std::max(numLocals, aPoint.getNumLocals()); 00116 return numPoints; 00117 } 00118 00120 unsigned int GblTrajectory::getNumPoints() const { 00121 return numPoints; 00122 } 00123 00125 00130 void GblTrajectory::addExternalSeed(unsigned int aLabel, 00131 const TMatrixDSym &aSeed) { 00132 externalPoint = aLabel; 00133 externalSeed.ResizeTo(aSeed); 00134 externalSeed = aSeed; 00135 } 00136 00138 00142 void GblTrajectory::defineOffsets() { 00143 // first point is offset 00144 thePoints[0].setOffset(0); 00145 int nOffsets = 1; 00146 // intermediate scatterers are offsets 00147 for (unsigned int i = 1; i < numPoints - 1; i++) { 00148 if (thePoints[i].hasScatterer()) { 00149 thePoints[i].setOffset(nOffsets); 00150 nOffsets++; 00151 } else { 00152 thePoints[i].setOffset(-nOffsets); 00153 } 00154 } 00155 // last point is offset 00156 thePoints[numPoints - 1].setOffset(nOffsets); 00157 numOffsets = nOffsets + 1; 00158 numParameters = numOffsets * theDimension.size() + numCurvature + numLocals; 00159 } 00160 00162 void GblTrajectory::calcJacobians() { 00163 00164 SMatrix55 scatJacobian; 00165 // forward propagation (all) 00166 unsigned int lastPoint = 0; 00167 unsigned int numStep = 0; 00168 for (unsigned int iPoint = 1; iPoint < numPoints; iPoint++) { 00169 if (numStep == 0) { 00170 scatJacobian = thePoints[iPoint].getP2pJacobian(); 00171 } else { 00172 scatJacobian = thePoints[iPoint].getP2pJacobian() * scatJacobian; 00173 } 00174 numStep++; 00175 thePoints[iPoint].addPrevJacobian(scatJacobian); // iPoint -> previous scatterer 00176 if (thePoints[iPoint].getOffset() >= 0) { 00177 thePoints[lastPoint].addNextJacobian(scatJacobian); // lastPoint -> next scatterer 00178 numStep = 0; 00179 lastPoint = iPoint; 00180 } 00181 } 00182 // backward propagation (without scatterers) 00183 numStep = 0; 00184 unsigned iPoint = numPoints - 1; 00185 for (unsigned int i = 1; i < numPoints - 1; i++) { 00186 iPoint--; 00187 if (thePoints[iPoint].getOffset() >= 0) { 00188 numStep = 0; 00189 continue; // skip offsets 00190 } 00191 if (numStep == 0) { 00192 scatJacobian = thePoints[iPoint].getP2pJacobian(); 00193 } else { 00194 scatJacobian = scatJacobian * thePoints[iPoint].getP2pJacobian(); 00195 } 00196 numStep++; 00197 thePoints[iPoint].addNextJacobian(scatJacobian); // iPoint -> next scatterer 00198 } 00199 } 00200 00202 00210 std::pair<std::vector<unsigned int>, TMatrixD> GblTrajectory::getJacobian( 00211 int aSignedLabel) const { 00212 00213 unsigned int nDim = theDimension.size(); 00214 unsigned int nCurv = numCurvature; 00215 unsigned int nLocals = numLocals; 00216 unsigned int nBorder = nCurv + nLocals; 00217 unsigned int nParBRL = nBorder + 2 * nDim; 00218 unsigned int nParLoc = nLocals + 5; 00219 std::vector<unsigned int> anIndex; 00220 anIndex.reserve(nParBRL); 00221 TMatrixD aJacobian(nParLoc, nParBRL); 00222 aJacobian.Zero(); 00223 00224 unsigned int aLabel = abs(aSignedLabel); 00225 int nJacobian; // 0: prev, 1: next 00226 // check consistency of (index, direction) 00227 if (aSignedLabel > 0) { 00228 nJacobian = 1; 00229 if (aLabel >= numPoints) { 00230 aLabel = numPoints; 00231 nJacobian = 0; 00232 } 00233 } else { 00234 nJacobian = 0; 00235 if (aLabel <= 0) { 00236 aLabel = 1; 00237 nJacobian = 1; 00238 } 00239 } 00240 GblPoint aPoint = thePoints[aLabel - 1]; 00241 std::vector<unsigned int> labDer(5); 00242 SMatrix55 matDer; 00243 getFitToLocalJacobian(labDer, matDer, aPoint, 5, nJacobian); 00244 00245 // from local parameters 00246 for (unsigned int i = 0; i < nLocals; i++) { 00247 aJacobian(i + 5, i) = 1.0; 00248 anIndex.push_back(i + 1); 00249 } 00250 // from trajectory parameters 00251 unsigned int iCol = nLocals; 00252 for (unsigned int i = 0; i < 5; i++) { 00253 if (labDer[i] > 0) { 00254 anIndex.push_back(labDer[i]); 00255 for (unsigned int j = 0; j < 5; j++) { 00256 aJacobian(j, iCol) = matDer(j, i); 00257 } 00258 iCol++; 00259 } 00260 } 00261 return std::make_pair(anIndex, aJacobian); 00262 } 00263 00265 00274 void GblTrajectory::getFitToLocalJacobian(std::vector<unsigned int> &anIndex, 00275 SMatrix55 &aJacobian, GblPoint &aPoint, unsigned int measDim, 00276 unsigned int nJacobian) const { 00277 00278 unsigned int nDim = theDimension.size(); 00279 unsigned int nCurv = numCurvature; 00280 unsigned int nLocals = numLocals; 00281 00282 int nOffset = aPoint.getOffset(); 00283 00284 if (nOffset < 0) // need interpolation 00285 { 00286 SMatrix22 prevW, prevWJ, nextW, nextWJ, sumWJ, matN, prevNW, nextNW; 00287 SVector2 prevWd, nextWd, prevNd, nextNd; 00288 int ierr; 00289 aPoint.getDerivatives(0, prevW, prevWJ, prevWd); // W-, W- * J-, W- * d- 00290 aPoint.getDerivatives(1, nextW, nextWJ, nextWd); // W-, W- * J-, W- * d- 00291 sumWJ = prevWJ + nextWJ; 00292 matN = sumWJ.Inverse(ierr); // N = (W- * J- + W+ * J+)^-1 00293 // derivatives for u_int 00294 prevNW = matN * prevW; // N * W- 00295 nextNW = matN * nextW; // N * W+ 00296 prevNd = matN * prevWd; // N * W- * d- 00297 nextNd = matN * nextWd; // N * W+ * d+ 00298 00299 unsigned int iOff = nDim * (-nOffset - 1) + nLocals + nCurv + 1; // first offset ('i' in u_i) 00300 00301 // local offset 00302 if (nCurv > 0) { 00303 aJacobian.Place_in_col(-prevNd - nextNd, 3, 0); // from curvature 00304 anIndex[0] = nLocals + 1; 00305 } 00306 aJacobian.Place_at(prevNW, 3, 1); // from 1st Offset 00307 aJacobian.Place_at(nextNW, 3, 3); // from 2nd Offset 00308 for (unsigned int i = 0; i < nDim; i++) { 00309 anIndex[1 + theDimension[i]] = iOff + i; 00310 anIndex[3 + theDimension[i]] = iOff + nDim + i; 00311 } 00312 00313 // local slope and curvature 00314 if (measDim > 2) { 00315 SMatrix22 prevWPN, nextWPN; 00316 SVector2 prevWNd, nextWNd; 00317 // derivatives for u'_int 00318 prevWPN = nextWJ * prevNW; // W+ * J+ * N * W- 00319 nextWPN = prevWJ * nextNW; // W- * J- * N * W+ 00320 prevWNd = nextWJ * prevNd; // W+ * J+ * N * W- * d- 00321 nextWNd = prevWJ * nextNd; // W- * J- * N * W+ * d+ 00322 if (nCurv > 0) { 00323 aJacobian(0, 0) = 1.0; 00324 aJacobian.Place_in_col(prevWNd - nextWNd, 1, 0); // from curvature 00325 } 00326 aJacobian.Place_at(-prevWPN, 1, 1); // from 1st Offset 00327 aJacobian.Place_at(nextWPN, 1, 3); // from 2nd Offset 00328 } 00329 } else { 00330 unsigned int iOff = nDim * (nOffset + nJacobian - 1) + nCurv + nLocals 00331 + 1; // first offset ('i' in u_i) 00332 00333 // local offset 00334 aJacobian(3, 1) = 1.0; // from 1st Offset 00335 aJacobian(4, 2) = 1.0; 00336 for (unsigned int i = 0; i < nDim; i++) { 00337 anIndex[1 + theDimension[i]] = iOff + i; 00338 } 00339 00340 // local slope and curvature 00341 if (measDim > 2) { 00342 SMatrix22 matW, matWJ; 00343 SVector2 vecWd; 00344 aPoint.getDerivatives(nJacobian, matW, matWJ, vecWd); // W, W * J, W * d 00345 if (nCurv > 0) { 00346 aJacobian(0, 0) = 1.0; 00347 aJacobian.Place_in_col(-vecWd, 1, 0); // from curvature 00348 anIndex[0] = nLocals + 1; 00349 } 00350 aJacobian.Place_at(-matWJ, 1, 1); // from 1st Offset 00351 aJacobian.Place_at(matW, 1, 3); // from 2nd Offset 00352 for (unsigned int i = 0; i < nDim; i++) { 00353 anIndex[3 + theDimension[i]] = iOff + nDim + i; 00354 } 00355 } 00356 } 00357 } 00358 00360 00366 void GblTrajectory::getFitToKinkJacobian(std::vector<unsigned int> &anIndex, 00367 SMatrix27 &aJacobian, GblPoint &aPoint) const { 00368 00369 unsigned int nDim = theDimension.size(); 00370 unsigned int nCurv = numCurvature; 00371 unsigned int nLocals = numLocals; 00372 00373 int nOffset = aPoint.getOffset(); 00374 00375 SMatrix22 prevW, prevWJ, nextW, nextWJ, sumWJ; 00376 SVector2 prevWd, nextWd, sumWd; 00377 aPoint.getDerivatives(0, prevW, prevWJ, prevWd); // W-, W- * J-, W- * d- 00378 aPoint.getDerivatives(1, nextW, nextWJ, nextWd); // W-, W- * J-, W- * d- 00379 sumWJ = prevWJ + nextWJ; // W- * J- + W+ * J+ 00380 sumWd = prevWd + nextWd; // W+ * d+ + W- * d- 00381 00382 unsigned int iOff = (nOffset - 1) * nDim + nCurv + nLocals + 1; // first offset ('i' in u_i) 00383 00384 // local offset 00385 if (nCurv > 0) { 00386 aJacobian.Place_in_col(-sumWd, 0, 0); // from curvature 00387 anIndex[0] = nLocals + 1; 00388 } 00389 aJacobian.Place_at(prevW, 0, 1); // from 1st Offset 00390 aJacobian.Place_at(-sumWJ, 0, 3); // from 2nd Offset 00391 aJacobian.Place_at(nextW, 0, 5); // from 1st Offset 00392 for (unsigned int i = 0; i < nDim; i++) { 00393 anIndex[1 + theDimension[i]] = iOff + i; 00394 anIndex[3 + theDimension[i]] = iOff + nDim + i; 00395 anIndex[5 + theDimension[i]] = iOff + nDim * 2 + i; 00396 } 00397 } 00398 00400 00408 void GblTrajectory::getResults(int aSignedLabel, TVectorD &localPar, 00409 TMatrixDSym &localCov) const { 00410 std::pair<std::vector<unsigned int>, TMatrixD> indexAndJacobian = 00411 getJacobian(aSignedLabel); 00412 unsigned int nParBrl = indexAndJacobian.first.size(); 00413 TVectorD aVec(nParBrl); // compressed vector 00414 for (unsigned int i = 0; i < nParBrl; i++) { 00415 aVec[i] = theVector(indexAndJacobian.first[i] - 1); 00416 } 00417 TMatrixDSym aMat = theMatrix.getBlockMatrix(indexAndJacobian.first); // compressed matrix 00418 localPar = indexAndJacobian.second * aVec; 00419 localCov = aMat.Similarity(indexAndJacobian.second); 00420 } 00421 00423 void GblTrajectory::buildLinearEquationSystem() { 00424 unsigned int nBorder = numCurvature + numLocals; 00425 theVector.resize(numParameters); 00426 theMatrix.resize(numParameters, nBorder); 00427 double aValue, aWeight; 00428 std::vector<unsigned int>* indLocal; 00429 std::vector<double>* derLocal; 00430 std::vector<GblData>::iterator itData; 00431 for (itData = theData.begin(); itData < theData.end(); itData++) { 00432 itData->getLocalData(aValue, aWeight, indLocal, derLocal); 00433 for (unsigned int j = 0; j < indLocal->size(); j++) { 00434 theVector((*indLocal)[j] - 1) += (*derLocal)[j] * aWeight * aValue; 00435 } 00436 theMatrix.addBlockMatrix(aWeight, indLocal, derLocal); 00437 } 00438 } 00439 00441 00444 void GblTrajectory::prepare() { 00445 unsigned int nDim = theDimension.size(); 00446 unsigned int maxData = 2 * (numPoints + numOffsets - 2); // upper limit 00447 theData.reserve(maxData); 00448 // measurements 00449 SMatrix55 matP; 00450 std::vector<GblPoint>::iterator itPoint; 00451 for (itPoint = thePoints.begin(); itPoint < thePoints.end(); itPoint++) { 00452 SVector5 aMeas, aPrec; 00453 unsigned int measDim = itPoint->hasMeasurement(); 00454 if (measDim) { 00455 unsigned int nLabel = itPoint->getLabel(); 00456 TMatrixD localDer = itPoint->getLocalDerivatives(); 00457 std::vector<int> globalLab = itPoint->getGlobalLabels(); 00458 TMatrixD globalDer = itPoint->getGlobalDerivatives(); 00459 itPoint->getMeasurement(matP, aMeas, aPrec); 00460 unsigned int iOff = 5 - measDim; // first active component 00461 std::vector<unsigned int> labDer(5); 00462 SMatrix55 matDer, matPDer; 00463 getFitToLocalJacobian(labDer, matDer, *itPoint, measDim); 00464 if (measDim > 2) { 00465 matPDer = matP * matDer; 00466 } else { // 'shortcut' for position measurements 00467 matPDer.Place_at( 00468 matP.Sub<SMatrix22>(3, 3) * matDer.Sub<SMatrix25>(3, 0), 00469 3, 0); 00470 } 00471 00472 for (unsigned int i = iOff; i < 5; i++) { 00473 if (aPrec(i) > 0.) { 00474 GblData aData(nLabel, aMeas(i), aPrec(i)); 00475 aData.addDerivatives(i, labDer, matPDer, iOff, localDer, 00476 globalLab, globalDer); 00477 theData.push_back(aData); 00478 } 00479 } 00480 00481 } 00482 } 00483 // pseudo measurements from kinks 00484 for (itPoint = thePoints.begin() + 1; itPoint < thePoints.end() - 1; 00485 itPoint++) { 00486 SVector2 aMeas, aPrec; 00487 if (itPoint->hasScatterer()) { 00488 unsigned int nLabel = itPoint->getLabel(); 00489 itPoint->getScatterer(aMeas, aPrec); 00490 std::vector<unsigned int> labDer(7); 00491 SMatrix27 matDer; 00492 getFitToKinkJacobian(labDer, matDer, *itPoint); 00493 for (unsigned int i = 0; i < nDim; i++) { 00494 unsigned int iDim = theDimension[i]; 00495 if (aPrec(iDim) > 0.) { 00496 GblData aData(nLabel, aMeas(iDim), aPrec(iDim)); 00497 aData.addDerivatives(iDim, labDer, matDer); 00498 theData.push_back(aData); 00499 } 00500 } 00501 } 00502 } 00503 // external seed 00504 if (externalPoint > 0) { 00505 std::pair<std::vector<unsigned int>, TMatrixD> indexAndJacobian = 00506 getJacobian(externalPoint); 00507 externalIndex = indexAndJacobian.first; 00508 std::vector<double> externalDerivatives(externalIndex.size()); 00509 TMatrixDSymEigen externalEigen(externalSeed); 00510 TVectorD valEigen = externalEigen.GetEigenValues(); 00511 TMatrixD vecEigen = externalEigen.GetEigenVectors(); 00512 vecEigen = vecEigen.T() * indexAndJacobian.second; 00513 for (int i = 0; i < externalSeed.GetNrows(); i++) { 00514 if (valEigen(i) > 0.) { 00515 for (int j = 0; j < externalSeed.GetNcols(); j++) { 00516 externalDerivatives[j] = vecEigen(i, j); 00517 } 00518 GblData aData(externalPoint, 0., valEigen(i)); 00519 aData.addDerivatives(externalIndex, externalDerivatives); 00520 theData.push_back(aData); 00521 } 00522 } 00523 } 00524 } 00525 00527 void GblTrajectory::predict() { 00528 std::vector<GblData>::iterator itData; 00529 for (itData = theData.begin(); itData < theData.end(); itData++) { 00530 itData->setPrediction(theVector); 00531 } 00532 } 00533 00535 00538 double GblTrajectory::downWeight(unsigned int aMethod) { 00539 double aLoss = 0.; 00540 std::vector<GblData>::iterator itData; 00541 for (itData = theData.begin(); itData < theData.end(); itData++) { 00542 aLoss += (1. - itData->setDownWeighting(aMethod)); 00543 } 00544 return aLoss; 00545 } 00546 00548 00557 unsigned int GblTrajectory::fit(double &Chi2, int &Ndf, double &lostWeight, 00558 std::string optionList) { 00559 const double normChi2[4] = { 1.0, 0.8737, 0.9326, 0.8228 }; 00560 const std::string methodList = "TtHhCc"; 00561 00562 unsigned int aMethod = 0; 00563 00564 defineOffsets(); 00565 calcJacobians(); 00566 prepare(); 00567 buildLinearEquationSystem(); 00568 lostWeight = 0.; 00569 unsigned int ierr = 0; 00570 try { 00571 00572 theMatrix.solveAndInvertBorderedBand(theVector, theVector); 00573 predict(); 00574 00575 for (unsigned int i = 0; i < optionList.size(); i++) // down weighting iterations 00576 { 00577 size_t aPosition = methodList.find(optionList[i]); 00578 if (aPosition != std::string::npos) { 00579 aMethod = aPosition / 2 + 1; 00580 lostWeight = downWeight(aMethod); 00581 buildLinearEquationSystem(); 00582 theMatrix.solveAndInvertBorderedBand(theVector, theVector); 00583 predict(); 00584 } 00585 } 00586 Ndf = theData.size() - numParameters; 00587 Chi2 = 0.; 00588 for (unsigned int i = 0; i < theData.size(); i++) { 00589 Chi2 += theData[i].getChi2(); 00590 } 00591 Chi2 /= normChi2[aMethod]; 00592 00593 } catch (int e) { 00594 std::cout << " GblTrajectory::fit exception " << e << std::endl; 00595 Chi2=0.; 00596 Ndf =-1; 00597 lostWeight = 0.; 00598 ierr = e; 00599 } 00600 return ierr; 00601 } 00602 00604 void GblTrajectory::milleOut(MilleBinary &aMille) { 00605 float fValue; 00606 float fErr; 00607 std::vector<unsigned int>* indLocal; 00608 std::vector<double>* derLocal; 00609 std::vector<int>* labGlobal; 00610 std::vector<double>* derGlobal; 00611 00612 // data: measurements, kinks and external seed 00613 std::vector<GblData>::iterator itData; 00614 for (itData = theData.begin(); itData < theData.end(); itData++) { 00615 itData->getAllData(fValue, fErr, indLocal, derLocal, labGlobal, 00616 derGlobal); 00617 aMille.addData(fValue, fErr, *indLocal, *derLocal, *labGlobal, 00618 *derGlobal); 00619 } 00620 aMille.writeRecord(); 00621 } 00622 00624 00628 typedef ROOT::Math::SMatrix<double, 5, 5, ROOT::Math::MatRepSym<double, 5> > SMatrixSym5; 00629 typedef ROOT::Math::SMatrix<double, 2, 2, ROOT::Math::MatRepSym<double, 2> > SMatrixSym2; 00630 typedef ROOT::Math::SMatrix<double, 5, 2> SMatrix52; 00631 00632 void GblTrajectory::kalmanGainFilter(double &Chi2, int &Ndf) { 00633 SVector5 state; 00634 SVector2 measurement, residuals; 00635 SMatrixSym5 covariance = ROOT::Math::SMatrixIdentity(); 00636 SMatrixSym2 covMeas, matSum, covRes; 00637 covariance = covariance * 1.0E4; 00638 SMatrix55 matP, aJacobian, mat1, mat1mKH; 00639 SMatrix25 matH; 00640 SMatrix52 matK; 00641 std::vector<SVector5> allStates; 00642 std::vector<SMatrix55> allCovs; 00643 allStates.reserve(numPoints); 00644 allCovs.reserve(numPoints); 00645 00646 Ndf = -5; 00647 Chi2 = 0.0; 00648 mat1 = ROOT::Math::SMatrixIdentity(); 00649 int iFail; 00650 00651 std::vector<GblPoint>::iterator itPoint; 00652 for (itPoint = thePoints.begin(); itPoint < thePoints.end(); itPoint++) { 00653 // propagate 00654 aJacobian = itPoint->getP2pJacobian(); 00655 state = aJacobian * state; 00656 covariance = ROOT::Math::Similarity(aJacobian, covariance); 00657 // scatterer? 00658 if (itPoint->hasScatterer()) { 00659 SVector2 aMeas, aPrec; 00660 itPoint->getScatterer(aMeas, aPrec); 00661 covariance(1, 1) += 1.0 / aPrec(0); 00662 covariance(2, 2) += 1.0 / aPrec(1); 00663 } 00664 // measurement, 2D only !!! 00665 unsigned int measDim = itPoint->hasMeasurement(); 00666 if (measDim == 2) { 00667 SVector5 aMeas, aPrec; 00668 itPoint->getMeasurement(matP, aMeas, aPrec); 00669 // update ndf, measurement precision matrix 00670 measurement(0) = aMeas(3); 00671 measurement(1) = aMeas(4); 00672 covMeas(0, 0) = 1.0 / aPrec(3); 00673 covMeas(1, 1) = 1.0 / aPrec(4); 00674 matH = matP.Sub<SMatrix25>(3, 0); 00675 Ndf += 2; 00676 // construct gain matrix 00677 iFail = 0; 00678 matSum = covMeas + ROOT::Math::Similarity(matH, covariance); 00679 matK = covariance * ROOT::Math::Transpose(matH) 00680 * matSum.InverseFast(iFail); 00681 00682 // residuals of prediction 00683 residuals = measurement - matH * state; 00684 // update of state 00685 state += matK * residuals; 00686 // update of covariance 00687 mat1mKH = mat1 - matK * matH; 00688 covariance = ROOT::Math::Similarity(mat1mKH, covariance) 00689 + ROOT::Math::Similarity(matK, covMeas); 00690 // filtered residuals 00691 residuals = measurement - matH * state; 00692 // covariance of filtered residuals 00693 covRes = covMeas - ROOT::Math::Similarity(matH, covariance); 00694 // Chi2 increment 00695 Chi2 += ROOT::Math::Similarity(residuals, 00696 covRes.InverseFast(iFail)); 00697 // save states and covs (for smoother) 00698 allStates.push_back(state); 00699 allCovs.push_back(covariance); 00700 } 00701 } 00702 } 00703
1.7.6.1
