-
-
-//---------------------------------------------------------------------
-void clitk::SignalMeanPositionFilter::ComputeIsoPhase(std::vector<clitk::Ellipse*> & l,
- std::vector<double> & phase,
- std::vector<int> & cycles) {
- double refphaseangle=0;
- double previousangle=0;
- phase.resize(mListOfEllipses.size());
-
- // DD(mListOfEllipses.size());
- mIsoPhaseIndex.resize(mListOfEllipses.size());
- // mIsoPhaseDelta.resize(mNumberOfIsoPhase);
- // mIsoPhaseDeltaNb.resize(mNumberOfIsoPhase);
- mIsoPhaseRefAngle.resize(mNumberOfIsoPhase);
-
- for (unsigned int i=0; i<mListOfEllipses.size(); i++) {
- clitk::Ellipse & An = *mListOfEllipses[i];
-
- // Current time accordint to initial input signal
- // int time = i+mAugmentationDelay+mWindowLength; // not use yet
-
- // Compute current signed angle
- Vector2d x1(An.ComputeCenter());
- double a = mListOfEllipses[0]->ComputeSemiAxeLengths()[0];
- double theta = mListOfEllipses[0]->ComputeAngleInRad();
- Vector2d x2; x2[0] = x1[0]+a * cos(theta); x2[1] = x1[1]+a * sin(theta);
- Vector2d x3(x1);
- Vector2d x4; x4[0] = mAugmentedInputX[i+mWindowLength]; x4[1] = mAugmentedInputY[i+mWindowLength];
- Vector2d A(x2-x1);
- Vector2d B(x4-x3);
- double signed_angle = atan2(B[1], B[0]) - atan2(A[1], A[0]);
- // double signed_angle = atan2(B[1], B[0]) - atan2(0, 1);
- if (signed_angle<0) signed_angle = 2*M_PI+signed_angle;
- // http://www.euclideanspace.com/maths/algebra/vectors/angleBetween/index.htm
-
- // First time : set the angle
- if (i==0) {
- refphaseangle = signed_angle;
- for(int a=0; a<mNumberOfIsoPhase; a++) {
- if (a==0) mIsoPhaseRefAngle[a] = signed_angle;
- else mIsoPhaseRefAngle[a] = (signed_angle
- + (2*M_PI)*a/mNumberOfIsoPhase);
- if (mIsoPhaseRefAngle[a] > 2*M_PI) mIsoPhaseRefAngle[a] -= 2*M_PI;
- if (mIsoPhaseRefAngle[a] < 0) mIsoPhaseRefAngle[a] = 2*M_PI-mIsoPhaseRefAngle[a];
- // DD(rad2deg(mIsoPhaseRefAngle[a]));
- // mIsoPhaseDelta[a] = 0.0;
- // mIsoPhaseDeltaNb[a] = 0;
- }
- int a=0;
- // DD(rad2deg(signed_angle));
- while ((a<mNumberOfIsoPhase) && (signed_angle>=mIsoPhaseRefAngle[a])) { a++; }
- // DD(a);
- mIsoPhaseIndex[i] = a-1;
- // DD(mIsoPhaseIndex[0]);
- cycles.push_back(0);
- }
- else {
- mIsoPhaseIndex[i] = mIsoPhaseIndex[i-1];
-
- // Check if angle cross a ref angle
- for(int a=0; a<mNumberOfIsoPhase; a++) {
- std::cout << "a=" << rad2deg(signed_angle) << " p=" << rad2deg(previousangle)
- << " ref=" << rad2deg(mIsoPhaseRefAngle[a]) << std::endl;
- if (
- (((signed_angle > mIsoPhaseRefAngle[a]) && (previousangle < mIsoPhaseRefAngle[a]))) ||
- ((mIsoPhaseRefAngle[a]==0) && (signed_angle < previousangle)))
- {
- // if (mValidationWithRealPhase) {
- // mIsoPhaseDelta[a] += mInputPhase[time];
- // mIsoPhaseDeltaNb[a]++;
- // }
- mIsoPhaseIndex[i] = a;
- // DD(a);
- cycles.push_back(i);
- }
- }
- // DD(mIsoPhaseIndex[i]);
- }
-
- previousangle = signed_angle;
- }
-
- /*
- if (mValidationWithRealPhase) {
- // Mean of all deltas
- for(unsigned int a=0; a<mIsoPhaseDelta.size(); a++) {
- if (mIsoPhaseDelta[a] == 0) {
- std::cerr << "Error : less than one cyle ?" << std::endl;
- exit(0);
- }
- mIsoPhaseDelta[a] /= mIsoPhaseDeltaNb[a];
- DD(mIsoPhaseDelta[a]);
- }
- std::ofstream os;
- openFileForWriting(os, "delta.sig");
- for(unsigned int a=0; a<mIsoPhaseDelta.size(); a++) {
- os << mIsoPhaseDelta[a] << std::endl;
- }
- os.close();
- }
- */
-
- /*
- if (mUseLearnedDeltaPhase) {
- for(unsigned int a=0; a<mIsoPhaseDelta.size(); a++) {
- mIsoPhaseDelta[a] = mLearnIsoPhaseDelta[a];
- DD(mIsoPhaseDelta[a]);
- }
- }
- */
-
- // DEBUG UNUSED
- /*
- int j=0;
- for (unsigned int i=0; i<cycles.size(); i++) {
- DD(cycles[i]);
- int n;
- if (i!=0) {
- n=cycles[i]-cycles[i-1];
- for(int index=0; index<n; index++) {
-
- int indexOfPhase1 = mIsoPhaseIndex[cycles[i-1]];
- int indexOfPhase2 = mIsoPhaseIndex[cycles[i]];
- DD(indexOfPhase1);
- DD(indexOfPhase2);
- double ph1 = mIsoPhaseDelta[indexOfPhase1];
- double ph2 = mIsoPhaseDelta[indexOfPhase2];
- DD(ph1);
- DD(ph2);
-
- phase[j] = (ph1+(double)index*(ph2-ph1)/(double)n);
- if (phase[j]>1.0) phase[j] = 1.0-phase[j];
- DD(phase[j]);
- j++;
- }
- }
- else j+=cycles[0];
- }
- */
- for (unsigned int i=0; i<cycles.size(); i++) {
- DD(cycles[i]);
- }
-
-}
-//---------------------------------------------------------------------
-