[chronojump] kneeAngle: done smooth points at end of capture

[Xavier de Blas <xaviblas src gnome org>]

commit f203dd714518e27b776fa857051a46268b741680
Author: Xavier de Blas <xaviblas gmail com>
Date:   Tue Apr 27 18:42:03 2010 +0800

    kneeAngle: done smooth points at end of capture

 src/angle/kneeAngle.cpp        |   40 ++++++++++++++++++++++-----
 src/angle/kneeAngleGlobal.cpp  |    8 +++++
 src/angle/kneeAngleRInside.cpp |   57 ++++++++++++++++++++++++++++++++++++++-
 3 files changed, 95 insertions(+), 10 deletions(-)
---
diff --git a/src/angle/kneeAngle.cpp b/src/angle/kneeAngle.cpp
index 02d4b4b..e787e3e 100644
--- a/src/angle/kneeAngle.cpp
+++ b/src/angle/kneeAngle.cpp
@@ -265,7 +265,7 @@ int main(int argc,char **argv)
 	FILE *fdatapost; //each line: 'current box height percent; current angle' (percent comes from fheader)
 
 	//file for smoothing and predictions
-//	FILE *fpointsdump; //contains X,Y of three points each frame
+	FILE *fpointsdump; //contains X,Y of three points each frame
 
 	char header[] = "_header.txt";
 	char txt[] = ".txt";
@@ -273,7 +273,7 @@ int main(int argc,char **argv)
 	char fheaderName [strlen(fileName) + strlen(header)];
 	char fdatapreName [strlen(fileName) + strlen(txt)];
 	char fdatapostName [strlen(fileName) + strlen(csv)];
-//	char fpointsdumpName[] = "pointsDump.csv";
+	char fpointsdumpName[] = "pointsDump.csv";
 
 	if(programMode == validation) {
 //		cvNamedWindow("Holes_on_contour",1);
@@ -316,17 +316,15 @@ int main(int argc,char **argv)
 	else if (programMode == blackWithoutMarkers)
 		cvNamedWindow("result",1);
 
-	/*
 	//put headers on pointsDump file	
 	if((fpointsdump=fopen(fpointsdumpName,"w"))==NULL){
 		printf("Error, no se puede escribir en el fichero %s\n",fpointsdumpName);
 		fclose(fpointsdump);
 		exit(0);
 	} else {
-		fprintf(fpointsdump, "hipX;hipY;kneeX;kneeY;toeX;toeY\n");
+		fprintf(fpointsdump, "hipX;hipY;kneeX;kneeY;toeX;toeY;hipXS;hipYS;kneeXS;kneeYS;toeXS;toeYS\n");
 		fclose(fpointsdump);
 	}
-	*/
 		
 	int kneePointWidth = -1;
 	int toePointWidth = -1;
@@ -750,14 +748,14 @@ imageGuiResult(gui, "returned", font);
 				fclose(fpointsdump);
 				*/
 				
-				if(usePrediction) {
+//				if(usePrediction) {
 					hipXVector.push_back(hipMarked.x);
 					hipYVector.push_back(hipMarked.y);
 					kneeXVector.push_back(kneeMarked.x);
 					kneeYVector.push_back(kneeMarked.y);
 					toeXVector.push_back(toeMarked.x);
 					toeYVector.push_back(toeMarked.y);
-				}
+//				}
 			}
 
 
@@ -2012,7 +2010,33 @@ imageGuiResult(gui, "returned", font);
 	do {
 		key =  (char) cvWaitKey(0);
 	} while (key != 'q' && key != 'Q');
-					
+				
+
+
+	//TODO: end at max angle. integrate with fdatapre, fdatapost
+
+	if(programMode == validation || programMode == blackWithoutMarkers) 
+	{
+		//'a' because initially we written the header row
+		if((fpointsdump=fopen(fpointsdumpName,"a"))==NULL){
+			printf("Error, no se puede añadir en el fichero %s\n",fpointsdumpName);
+		} else {
+			//find smoothed vectors:			
+			smoothPoints();
+
+			//print all data:
+			for (int i=0; i < hipXVector.size(); i ++) {
+				fprintf(fpointsdump, "%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d\n", 
+						hipXVector[i], hipYVector[i],
+						kneeXVector[i], kneeYVector[i],
+						toeXVector[i], toeYVector[i],
+						hipXVectorS[i], hipYVectorS[i],
+						kneeXVectorS[i], kneeYVectorS[i],
+						toeXVectorS[i], toeYVectorS[i]);
+	      		}
+		}
+		fclose(fpointsdump);
+	}
 			
 
 	cvClearMemStorage( stickStorage );
diff --git a/src/angle/kneeAngleGlobal.cpp b/src/angle/kneeAngleGlobal.cpp
index 22015e1..de76ca2 100644
--- a/src/angle/kneeAngleGlobal.cpp
+++ b/src/angle/kneeAngleGlobal.cpp
@@ -125,3 +125,11 @@ std::vector<int> kneeYVector;
 std::vector<int> toeXVector;
 std::vector<int> toeYVector;
 
+//smoothed
+std::vector<int> hipXVectorS;
+std::vector<int> hipYVectorS;
+std::vector<int> kneeXVectorS;
+std::vector<int> kneeYVectorS;
+std::vector<int> toeXVectorS;
+std::vector<int> toeYVectorS;
+
diff --git a/src/angle/kneeAngleRInside.cpp b/src/angle/kneeAngleRInside.cpp
index d654b75..5fbeb5d 100644
--- a/src/angle/kneeAngleRInside.cpp
+++ b/src/angle/kneeAngleRInside.cpp
@@ -25,10 +25,12 @@
 #include <string>
 
 
+/* ------------ PREDICT NEXT POINT (filtering outliers) --------------*/
+
 SEXP ansP;
 std::string txtP = "";
 
-void createPredictFunction()
+void createPredictNextFunction()
 {
 	//don't put comments '#'
 	txtP = "span = 30;	\
@@ -71,7 +73,7 @@ int predictDo(std::vector<int> vect) {
 
 CvSeq* predictPoints() {
 	if(txtP=="") 
-		createPredictFunction();
+		createPredictNextFunction();
 	
 	CvMemStorage* storage = cvCreateMemStorage(0);
 	CvSeq* seqPoints = cvCreateSeq( CV_SEQ_KIND_GENERIC|CV_32SC2, sizeof(CvSeq), sizeof(CvPoint), storage );
@@ -94,3 +96,54 @@ CvSeq* predictPoints() {
 	return seqPoints;
 }
 
+/* ------------ SMOOTH ALL POINTS (filtering outliers) --------------*/
+
+
+SEXP ansS;
+std::string txtS = "";
+
+void createSmoothFunction()
+{
+	//don't put comments '#'
+	txtS = "span = 30 / length(x);	\
+		threshold = 3;		\
+		discard = 10;	\
+				\
+		if( length(x) < discard || sd(x) == 0) {			\
+			n <- x[ length( x ) ];					\
+		} else {							\
+   			time <- 1:length(x); 					\
+										\
+			resid <- x - predict( loess(x ~ time, span = span), time );	\
+			x.outlier <- abs(resid) > threshold * sd(resid);	\
+										\
+			time.1   <- time[!x.outlier];				\
+			x.1      <- x   [!x.outlier];				\
+										\
+			n <- predict( loess(x.1 ~ time.1, span = span), time ); \
+		}		\
+		";
+
+	R.assign( txtS, "txtS"); 
+}
+
+std::vector<int> smoothDo(std::vector<int> vect) {
+//	if( ! vect.empty() ) {
+		R.assign(vect, "x");
+		R.parseEval(txtS, ansS);
+		return(Rcpp::as< std::vector< int > >(ansS));
+//	} else return 0;
+}
+
+void smoothPoints() {
+	if(txtS=="") 
+		createSmoothFunction();
+	
+	hipXVectorS = smoothDo(hipXVector);
+	hipYVectorS = smoothDo(hipYVector);
+	kneeXVectorS = smoothDo(kneeXVector);
+	kneeYVectorS = smoothDo(kneeYVector);
+	toeXVectorS = smoothDo(toeXVector);
+	toeYVectorS = smoothDo(toeYVector);
+}
+