/*=========================================================================
Program: clitk
+ Module: $RCSfile: clitkEllipse.cxx,v $
Language: C++
+ Date: $Date: 2010/03/03 10:47:48 $
+ Version: $Revision: 1.3 $
Copyright (c) CREATIS (Centre de Recherche et d'Applications en Traitement de
l'Image). All rights reserved. See Doc/License.txt or
http://www.creatis.insa-lyon.fr/Public/Gdcm/License.html for details.
- This software is distributed WITHOUT ANY WARRANTY; without even
- the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
- PURPOSE. See the above copyright notices for more information.
+ This software is distributed WITHOUT ANY WARRANTY; without even
+ the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ PURPOSE. See the above copyright notices for more information.
-=========================================================================*/
+ =========================================================================*/
#include "clitkEllipse.h"
clitk::Ellipse::Ellipse(const Ellipse & e):a((*this)[0]), b((*this)[1]),
c((*this)[2]), d((*this)[3]),
e((*this)[4]), f((*this)[5]) {
- for(int i=0; i<7; i++) (*this)[i] = e[i];
+ for(int i=0; i<6; i++) (*this)[i] = e[i];
}
//---------------------------------------------------------------------
Vector2d center = ComputeCenter();
double & k1 = center[0];
double & k2 = center[1];
+ // DD(f);
double mu = 1.0/(a*k1*k1 + b*k1*k2 + c*k2*k2 - f);
+ // DD(a*k1*k1);
+ // DD(b*k1*k2);
+ // DD(c*k2*k2);
+ // DD(a*k1*k1 + b*k1*k2 + c*k2*k2 - f);
+ // DD(mu);
double m11 = mu * a;
double m12 = mu * 0.5 * b;
double m22 = mu * c;
double l1 = ( (m11+m22) + sqrt((m11-m22)*(m11-m22)+4*m12*m12) )/2.0;
- assert(l1>0.0);
+ // DD(k1);
+ // DD(k2);
+ // DD(mu);
+ // DD(m11);
+ // DD(m12);
+ // DD(m22);
+ // DD(l1);
+ assert(l1>=0.0);
axis[1] = 1.0/sqrt(l1);
double l2 = ((m11+m22)-sqrt((m11-m22)*(m11-m22)+4*m12*m12))/2.0;
+ // DD(l2);
assert(l2>0.0);
axis[0] = 1.0/sqrt(l2);
return axis;
// residual r (no need to optimize)
GetVnlVector().normalize();
double r = (St*current)*current;
+ // DD(r);
// Temporary parameters
Vector6d an;
// Initialisation of S
S.Fill(0.0);
- j = 0;
- for(unsigned int i=begin; i<begin+n; i++) {
+ // j = 0;
+ for(unsigned int i=0; i<n; i++) {
for(unsigned int x=0; x<6; x++)
for(unsigned int y=0; y<6; y++)
- S(x,y) += z[j][x]*z[j][y];
- j++;
+ S(x,y) += z[i][x]*z[i][y];
+ // j++;
}
Sinv = S.GetInverse();
St = S.GetVnlMatrix().transpose();
}
}
//---------------------------------------------------------------------
+
+
+//---------------------------------------------------------------------
+void clitk::ComputeIsoPhaseFromListOfEllipses(std::vector<clitk::Ellipse*> & l,
+ clitk::Signal & sx,
+ clitk::Signal & sy,
+ int nbIsoPhase,
+ int delay,
+ int L,
+ clitk::Signal & phase) {
+ // Init
+ DD(nbIsoPhase);
+ std::vector<double> mIsoPhaseRefAngle(nbIsoPhase);
+ phase.resize(l.size());
+ double refphaseangle=0;
+ double previousangle=0;
+
+ // Loop on ellipses
+ // DD(l.size());
+ for (unsigned int i=0; i<l.size(); i++) {
+ // DD("=================================");
+ // DD(i);
+ clitk::Ellipse & An = *l[i];
+ // DD(An);
+ // DD(An.ComputeCenter());
+ // DD(An.ComputeSemiAxeLengths());
+ // DD(rad2deg(An.ComputeAngleInRad()));
+
+ // Compute current signed angle
+ Vector2d x1(An.ComputeCenter());
+ double a = l[0]->ComputeSemiAxeLengths()[0];
+ double theta = l[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] = sx[i+L]; x4[1] = sy[i+L];
+ Vector2d A(x2-x1);
+ Vector2d B(x4-x3);
+ // DD(sx[i+L]);
+ // DD(sy[i+L]);
+ // http://www.euclideanspace.com/maths/algebra/vectors/angleBetween/index.htm
+ double signed_angle = atan2(B[1], B[0]) - atan2(A[1], A[0]);
+ if (signed_angle<0) signed_angle = 2*M_PI+signed_angle;
+
+ // First time : set the angle
+ if (i==0) {
+ // DD(signed_angle);
+ refphaseangle = signed_angle;
+ for(int a=0; a<nbIsoPhase; a++) {
+ if (a==0) mIsoPhaseRefAngle[a] = refphaseangle;//signed_angle;
+ else mIsoPhaseRefAngle[a] = (refphaseangle //signed_angle
+ + (2*M_PI)*a/nbIsoPhase);
+ if (mIsoPhaseRefAngle[a] > 2*M_PI) mIsoPhaseRefAngle[a] -= 2*M_PI;
+ if (mIsoPhaseRefAngle[a] < 0) mIsoPhaseRefAngle[a] = 2*M_PI-mIsoPhaseRefAngle[a];
+ }
+ int a=0;
+ while ((a<nbIsoPhase) && (signed_angle>=mIsoPhaseRefAngle[a])) { a++; }
+ phase[i] = a-1;
+ if (nbIsoPhase == 1) phase[i] = 1;
+ }
+ else {
+ phase[i] = phase[i-1];
+
+ // Check if angle cross a ref angle
+ for(int a=0; a<nbIsoPhase; a++) {
+ // std::cout << "a=" << rad2deg(signed_angle) << " prev=" << rad2deg(previousangle)
+ // << " ref=" << rad2deg(mIsoPhaseRefAngle[a]) << " " << phase[i] << std::endl;
+ if (signed_angle > previousangle) {
+ // DD("cas1");
+ // (((signed_angle > mIsoPhaseRefAngle[a]) && (previousangle < mIsoPhaseRefAngle[a]))) ||
+ // ((mIsoPhaseRefAngle[a]==0) && (signed_angle < previousangle)))
+ if ((previousangle < mIsoPhaseRefAngle[a]) &&
+ (signed_angle >= mIsoPhaseRefAngle[a]))
+ {
+ // DD(a);
+ if (nbIsoPhase == 1) { // single phase, alternate 0 and 1
+ phase[i] = -phase[i-1];
+ }
+ else phase[i] = a;
+ }
+ }
+ else { // previousangle >= signed_angle (we turn around 0)
+ // DD("cas2");
+ if ((mIsoPhaseRefAngle[a] > previousangle) ||
+ (mIsoPhaseRefAngle[a] < signed_angle)) {
+ // DD(a);
+ if (nbIsoPhase == 1) { // single phase, alternate 0 and 1
+ phase[i] = -phase[i-1];
+ }
+ else phase[i] = a;
+ }
+ }
+ }
+ }
+ previousangle = signed_angle;
+ }
+}
+//---------------------------------------------------------------------
+
/*=========================================================================
Program: clitk
+ Module: $RCSfile: clitkSignalMeanPositionFilter.cxx,v $
Language: C++
+ Date: $Date: 2010/03/03 10:47:48 $
+ Version: $Revision: 1.6 $
Copyright (c) CREATIS (Centre de Recherche et d'Applications en Traitement de
l'Image). All rights reserved. See Doc/License.txt or
mAugmentationDelay = args_info.delay_arg;
mIsAdaptiveMethod = false;
mWindowLength = -1;
+ mValidationWithRealPhase = false;
+ mUseLearnedDeltaPhase = false;
if (args_info.eta_given) {
mEta = args_info.eta_arg;
mEtaIsSet = true;
mIsAdaptiveMethod = true;
mWindowLength = args_info.L_arg;
}
+ if (args_info.phase_given) {
+ mValidationWithRealPhase = true;
+ mInputPhaseFilename = args_info.phase_arg;
+ mInputPhase.Read(mInputPhaseFilename);
+ }
+ mNumberOfIsoPhase = args_info.nbiso_arg;
+ if (args_info.delta_given) {
+ mUseLearnedDeltaPhase = true;
+ mLearnIsoPhaseDelta.Read(args_info.delta_arg);
+ mNumberOfIsoPhase = mLearnIsoPhaseDelta.size();
+ }
+
+ // DEBUG
+ if (args_info.phase_given) {
+ std::cout << "PLEASE DO NO USE THIS OPTION --phase YET ..." << std::endl;
+ exit(0);
+ }
+ if (args_info.delta_given) {
+ std::cout << "PLEASE DO NO USE THIS OPTION --delta YET ..." << std::endl;
+ exit(0);
+ }
+
}
//---------------------------------------------------------------------
//---------------------------------------------------------------------
void clitk::SignalMeanPositionFilter::Update() {
- // DEBUG
-// int e = 5;
-// clitk::Signal temp;
-// temp.resize(mInput.size()*e);
-// for(unsigned int i=0; i<temp.size(); i++) {
-// int index = lrint(floor((double)i/e));
-// double res = (double)i/e - index;
-// // DD(index);
-// //DD(res);
-// temp[i] = mInput[index] + res*(mInput[index+1]-mInput[index])/e;
-// }
-// mInput.resize(temp.size());
- for (unsigned int i=0; i<mInput.size(); i++) mInput[i] = mInput[i]+(double)i/(double)mInput.size();
- srand ( time(0) );
- for (unsigned int i=0; i<mInput.size(); i++) {
- mInput[i] = (0.8+((double)rand()/ (double)RAND_MAX)*0.4) * mInput[i];
- }
-
- {
- std::ofstream os;
- openFileForWriting(os, "input.dat");
- for(unsigned int i=0; i<mInput.size(); i++) os << mInput[i] << std::endl;
- os.close();
- }
-
-
// Compute augmented space
if (mVerbose) {
std::cout << "Computing augmented space with delay = " << mAugmentationDelay
<< ", number of samples is " << mInput.size()-mAugmentationDelay
<< std::endl;
}
- ComputeAugmentedSpace(mInput, mAugmentedInputX, mAugmentedInputY, mAugmentationDelay);
+ mInput.ComputeAugmentedSpace(mAugmentedInputX, mAugmentedInputY, mAugmentationDelay);
// Output augmented state
if (mOutputAugmentedFilenameIsSet) {
std::ofstream os;
openFileForWriting(os, mOutputAugmentedFilename);
- for(unsigned int i=0; i<(unsigned int)mWindowLength; i++) {
+ for(unsigned int i=0; i<(unsigned int)mAugmentedInputX.size(); i++) {
os << mAugmentedInputX[i] << " " << mAugmentedInputY[i] << std::endl;
}
os.close();
// Initial starting ellipse
Ellipse An; // starting point
if (!mEtaIsSet) mEta = -1;
+ DD(mWindowLength);
+ if (mWindowLength == -1) {
+ mWindowLength = mAugmentedInputY.size();
+ }
An.InitialiseEllipseFitting(mEta, mWindowLength, mAugmentedInputX, mAugmentedInputY);
mEta = An.GetEta();
if (mVerbose) {
// Fitting method
if (mIsAdaptiveMethod) {
AdaptiveFitEllipse(An);
- //FitEllipse(An);
}
else FitEllipse(An);
if (mOutputFilenameIsSet) {
std::ofstream os;
openFileForWriting(os, mOutputFilename);
- os << AnInitial.ComputeCenter()[0] << " " << AnInitial.ComputeCenter()[1] << " "
- << AnInitial.ComputeSemiAxeLengths()[0] << " " << AnInitial.ComputeSemiAxeLengths()[1] << " "
- << AnInitial.ComputeAngleInRad();
- for(int i=0; i<6; i++) os << AnInitial[i] << " " ;
- os << std::endl;
- os << An.ComputeCenter()[0] << " " << An.ComputeCenter()[1] << " "
- << An.ComputeSemiAxeLengths()[0] << " " << An.ComputeSemiAxeLengths()[1] << " "
- << An.ComputeAngleInRad();
- for(int i=0; i<6; i++) os << An[i] << " ";
- os << std::endl;
- os.close();
+ std::vector<double> phase;
+ ComputeIsoPhase(mListOfEllipses, phase, mCycles);
+
+ //int currentCycle = 0;
+ for (unsigned int i=0; i<mListOfEllipses.size(); i++) {
+ clitk::Ellipse & An = *mListOfEllipses[i];
+ int time = i+mAugmentationDelay+mWindowLength;
+ os << time << " " // current 'time' in input
+ << mInput[time] << " " // current value in input
+ << An.ComputeCenter()[0] << " " << An.ComputeCenter()[1] << " "
+ << An.ComputeSemiAxeLengths()[0] << " " << An.ComputeSemiAxeLengths()[1] << " "
+ << An.ComputeAngleInRad() << " "
+ << rad2deg(An.ComputeAngleInRad()) << " _Phase_ "
+ << mIsoPhaseIndex[i] << " " ;
+
+ /*
+ if (mUseLearnedDeltaPhase) {
+ os << mIsoPhaseDelta[mIsoPhaseIndex[i]] << " " << phase[i] << " ";
+ }
+ */
- openFileForWriting(os, "centers.dat");
- for(unsigned int i=0; i<mCenters.size(); i++) {
- os << mCenters[i][0] << " " << mCenters[i][1] << std::endl;
+ os << " _Ellipse_ ";
+ for(int j=0; j<6; j++) os << An[j] << " ";
+ os << std::endl;
}
os.close();
-
}
if (mOutputResidualFilenameIsSet) {
//---------------------------------------------------------------------
-//---------------------------------------------------------------------
-void clitk::SignalMeanPositionFilter::ComputeAugmentedSpace(const clitk::Signal & input,
- clitk::Signal & outputX,
- clitk::Signal & outputY,
- unsigned int delay) {
- if (input.size() <= delay) {
- std::cerr << "Error in signal length is " << input.size()
- << " while delay is " << delay << " : too short. Abort." << std::endl;
- exit(0);
- }
- outputX.resize(input.size()-delay);
- outputY.resize(input.size()-delay);
- for(unsigned int i=0; i<outputX.size(); i++) {
- outputX[i] = input[i+delay];
- outputY[i] = input[i];
- }
-}
-
-//---------------------------------------------------------------------
-
-
//---------------------------------------------------------------------
void clitk::SignalMeanPositionFilter::FitEllipse(clitk::Ellipse & An) {
int n = 0;
//---------------------------------------------------------------------
void clitk::SignalMeanPositionFilter::AdaptiveFitEllipse(clitk::Ellipse & An) {
- for(unsigned int t=0; t<(unsigned int)args_info.t_arg; t++) {
- DD(t);
+ // Compute the total number of iteration :
+ // = nb of inputs minus the delay, minus the windowL, minus the first one
+ int maxN = std::min((unsigned int)args_info.t_arg,
+ mInput.size()-mWindowLength-mAugmentationDelay);
+ for(int t=0; t<maxN; t++) {
An.UpdateSMatrix(t, mWindowLength, mAugmentedInputX, mAugmentedInputY);
- mCenters.push_back(An.ComputeCenter());
FitEllipse(An);
+ clitk::Ellipse * B = new clitk::Ellipse(An);
+ mListOfEllipses.push_back(B);
}
}
//---------------------------------------------------------------------
+
+
+//---------------------------------------------------------------------
+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]);
+ }
+
+}
+//---------------------------------------------------------------------
+
git://git.creatis.insa-lyon.fr / clitk.git / commitdiff