[chronojump] kneeAngle prediction cleaner

[Xavier de Blas <xaviblas src gnome org>]

commit e9afe3630545d04c83db5b41fdb3e2de3289660a
Author: Xavier de Blas <xaviblas gmail com>
Date:   Tue Apr 27 01:52:10 2010 +0800

    kneeAngle prediction cleaner

 src/angle/kneeAngle.cpp        |   59 ++++++++++-----
 src/angle/kneeAngleGlobal.cpp  |    3 +-
 src/angle/kneeAngleRInside.cpp |  156 +++++++++++++---------------------------
 3 files changed, 92 insertions(+), 126 deletions(-)
---
diff --git a/src/angle/kneeAngle.cpp b/src/angle/kneeAngle.cpp
index 0027835..02d4b4b 100644
--- a/src/angle/kneeAngle.cpp
+++ b/src/angle/kneeAngle.cpp
@@ -151,7 +151,7 @@
 #include "kneeAngleGlobal.cpp"
 #include "kneeAngleUtil.cpp"
 #include "kneeAngleFunctions.cpp"
-#include "kneeAngleSystemCalls.cpp"
+#include "kneeAngleRInside.cpp"
 
 
 //using namespace std;
@@ -613,13 +613,26 @@ int main(int argc,char **argv)
 		
 
 		//predict where will be the points now
-		seqPredicted = predictPointsRInside();
-		hipPredicted = *CV_GET_SEQ_ELEM( CvPoint, seqPredicted, 0); 
-		kneePredicted = *CV_GET_SEQ_ELEM( CvPoint, seqPredicted, 1); 
-		toePredicted = *CV_GET_SEQ_ELEM( CvPoint, seqPredicted, 2); 
-		
-//		printf("%d;%d;%d;%d;%d;%d\n", hipPredicted.x, hipPredicted.y, 
-//			kneePredicted.x, kneePredicted.y, toePredicted.x, toePredicted.y);
+		if(usePrediction) {
+			seqPredicted = predictPoints();
+			hipPredicted = *CV_GET_SEQ_ELEM( CvPoint, seqPredicted, 0); 
+			kneePredicted = *CV_GET_SEQ_ELEM( CvPoint, seqPredicted, 1); 
+			toePredicted = *CV_GET_SEQ_ELEM( CvPoint, seqPredicted, 2); 
+		} else {
+			hipPredicted = hipOld;
+			kneePredicted = kneeOld;
+			toePredicted = toeOld;
+		}
+	
+		//uncomment to show how prediction works
+		/*
+		if(usePrediction) {
+			printf("1 OLD:  %d;%d;%d;%d;%d;%d\n", hipOld.x, hipOld.y, 
+					kneeOld.x, kneeOld.y, toeOld.x, toeOld.y);
+			printf("2 PRED: %d;%d;%d;%d;%d;%d\n", hipPredicted.x, hipPredicted.y, 
+					kneePredicted.x, kneePredicted.y, toePredicted.x, toePredicted.y);
+		}
+		*/
 
 
 		if(programMode == skinOnlyMarkers || programMode == blackOnlyMarkers || programMode == validation) 
@@ -737,17 +750,17 @@ imageGuiResult(gui, "returned", font);
 				fclose(fpointsdump);
 				*/
 				
-				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);
+				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);
+				}
 			}
 
 
-			
-
 // kalman 
 			measurement_pt = kneeMarked;
 
@@ -1701,10 +1714,16 @@ imageGuiResult(gui, "returned", font);
 					maxrect = findLargestContour(segmented, outputTemp, showContour);
 					
 					//predict where will be the points now
-					seqPredicted = predictPointsRInside();
-					hipPredicted = *CV_GET_SEQ_ELEM( CvPoint, seqPredicted, 0); 
-					kneePredicted = *CV_GET_SEQ_ELEM( CvPoint, seqPredicted, 1); 
-					toePredicted = *CV_GET_SEQ_ELEM( CvPoint, seqPredicted, 2); 
+					if(usePrediction) {
+						seqPredicted = predictPoints();
+						hipPredicted = *CV_GET_SEQ_ELEM( CvPoint, seqPredicted, 0); 
+						kneePredicted = *CV_GET_SEQ_ELEM( CvPoint, seqPredicted, 1); 
+						toePredicted = *CV_GET_SEQ_ELEM( CvPoint, seqPredicted, 2); 
+					} else {
+						hipPredicted = hipOld;
+						kneePredicted = kneeOld;
+						toePredicted = toeOld;
+					}
 
 					
 					findHoles(
diff --git a/src/angle/kneeAngleGlobal.cpp b/src/angle/kneeAngleGlobal.cpp
index 4f3bed6..22015e1 100644
--- a/src/angle/kneeAngleGlobal.cpp
+++ b/src/angle/kneeAngleGlobal.cpp
@@ -115,7 +115,8 @@ bool zoomed = false;
 double zoomScale = 2; 
 
 //predictions stuff
-RInside R = RInside();              // create an embedded R instance 
+bool usePrediction = false;	//unneded at 300 fps
+RInside R = RInside();		// create an embedded R instance 
 	
 std::vector<int> hipXVector;
 std::vector<int> hipYVector;
diff --git a/src/angle/kneeAngleRInside.cpp b/src/angle/kneeAngleRInside.cpp
index 1062818..d654b75 100644
--- a/src/angle/kneeAngleRInside.cpp
+++ b/src/angle/kneeAngleRInside.cpp
@@ -25,121 +25,67 @@
 #include <string>
 
 
-CvSeq* predictPointsRInside()
+SEXP ansP;
+std::string txtP = "";
+
+void createPredictFunction()
 {
-	/* TESTING:
+	//don't put comments '#'
+	txtP = "span = 30;	\
+		threshold = 3;	\
+		discard = 10;	\
+				\
+		if( length(x) < discard || sd(x) == 0) {			\
+			n<- x[ length( x ) ];					\
+		} else {							\
+			which.predict <- max( 1, length(x) - span):length(x);	\
+			time.1 <- 1:length( which.predict );			\
+			x.1    <- x[ which.predict ];				\
+										\
+			resid  <- x.1 - predict( lm( x.1 ~ time.1 ), data.frame( time.1 = time.1 ) );	\
+			x.outlier <- abs( resid ) > threshold * sd( resid );	\
+										\
+			time.1   <- time.1[!x.outlier];				\
+			x.1      <- x.1   [!x.outlier];				\
+										\
+			if( is.na( sd(x.1) ) ) {				\
+				n<-  x[ length( x ) ];				\
+			} else {						\
+				n<- round (					\
+				predict( lm( x.1 ~ time.1 ), newdata = data.frame( time.1 = length( time.1 ) + 1 ) ) 	\
+				);						\
+			}							\
+		}		\
+		";
 
-	R.assign(hipXVector, "d1");
-	R.assign(hipYVector, "d2");
-	R.assign(kneeXVector, "d3");
-	R.assign(kneeYVector, "d4");
-	R.assign(toeXVector, "d5");
-	R.assign(toeYVector, "d6");
-	std::string prova =               // now access in R
-		"cat('\nd1=', d1, '\n');"
-		"cat('\nd2=', d2, '\n');"
-		"cat('\nd3=', d3, '\n');"
-		"cat('\nd4=', d4, '\n');"
-		"cat('\nd5=', d5, '\n');"
-		"cat('\nd6=', d6, '\n');";
-	R.parseEvalQ(prova);
-	*/
+	R.assign( txtP, "txtP"); 
+}
 
+int predictDo(std::vector<int> vect) {
+	if( ! vect.empty() ) {
+		R.assign(vect, "x");
+		R.parseEval(txtP, ansP);
+		return(Rcpp::as< int >(ansP));
+	} else return 0;
+}
+
+CvSeq* predictPoints() {
+	if(txtP=="") 
+		createPredictFunction();
+	
 	CvMemStorage* storage = cvCreateMemStorage(0);
 	CvSeq* seqPoints = cvCreateSeq( CV_SEQ_KIND_GENERIC|CV_32SC2, sizeof(CvSeq), sizeof(CvPoint), storage );
 	CvPoint hip;
-	hip.x =0; hip.y=0;
 	CvPoint knee;
-	knee.x =0; knee.y=0;
 	CvPoint toe;
-	toe.x =0; toe.y=0;
-
-	SEXP ans;
-	std::string txt = "			\
-		span = 30;			\
-		threshold = 3;			\
-		discard = 10;			\
-						\
-		maxX = 512;			\
-		maxY = 384;			\
-		#cat('(', length(x), '): ', x[length(x)]);		\
-												\
-		if( length(x) < discard || sd(x) == 0) {					\
-			n<- x[ length( x ) ];							\
-		} else {									\
-			which.predict <- max( 1, length(x) - span):length(x);			\
-			time.1 <- 1:length( which.predict );					\
-			x.1    <- x[ which.predict ];						\
-												\
-			resid  <- x.1 - predict( lm( x.1 ~ time.1 ), data.frame( time.1 = time.1 ) );	\
-			x.outlier <- abs( resid ) > threshold * sd( resid );			\
-												\
-			time.1   <- time.1[!x.outlier];						\
-			x.1      <- x.1   [!x.outlier];						\
-												\
-			if( is.na( sd(x.1) ) ) {						\
-				n<-  x[ length( x ) ];						\
-			} else {								\
-				n<- round (							\
-				predict( lm( x.1 ~ time.1 ), newdata = data.frame( time.1 = length( time.1 ) + 1 ) ) 	\
-				);								\
-			}									\
-		}				\
-		";
-
-	R.assign( txt, "txt"); 
 	
-	//HIP
-	if(hipXVector.empty())
-		hip.x = -1;
-	else {
-		R.assign(hipXVector, "x");
-		R.parseEval(txt, ans);
-		hip.x = Rcpp::as< int >(ans);
-	}
+	hip.x =  predictDo(hipXVector);
+	hip.y =  predictDo(hipYVector);
+	knee.x = predictDo(kneeXVector);
+	knee.y = predictDo(kneeYVector);
+	toe.x =  predictDo(toeXVector);
+	toe.y =  predictDo(toeYVector);
 	
-	if(hipYVector.empty())
-		hip.y = -1;
-	else {
-		R.assign(hipYVector, "x");
-		R.parseEval(txt, ans);
-		hip.y = Rcpp::as< int >(ans);
-	}
-
-	//KNEE
-	if(kneeXVector.empty())
-		knee.x = -1;
-	else {
-		R.assign(kneeXVector, "x");
-		R.parseEval(txt, ans);
-		knee.x = Rcpp::as< int >(ans);
-	}
-
-	if(kneeYVector.empty())
-		knee.y = -1;
-	else {
-		R.assign(kneeYVector, "x");
-		R.parseEval(txt, ans);
-		knee.y = Rcpp::as< int >(ans);
-	}
-
-	//TOE
-	if(toeXVector.empty())
-		toe.x = -1;
-	else {
-		R.assign(toeXVector, "x");
-		R.parseEval(txt, ans);
-		toe.x = Rcpp::as< int >(ans);
-	}
-
-	if(toeYVector.empty())
-		toe.y = -1;
-	else {
-		R.assign(toeYVector, "x");
-		R.parseEval(txt, ans);
-		toe.y = Rcpp::as< int >(ans);
-	}
-		
 	//PASS to cvSeq	
 	cvSeqPush( seqPoints, &hip );
 	cvSeqPush( seqPoints, &knee );