GNOME.org
 

[chronojump] RInside for predict points

commit 57e5da7a82bf6b938a4d3cb01a0eeee61f03e43f
Author: Xavier de Blas <xaviblas gmail com>
Date:   Mon Apr 26 19:09:48 2010 +0800

    RInside for predict points

 src/angle/kneeAngle.cpp          |   98 +++++++++++++++++++++----
 src/angle/kneeAngleFunctions.cpp |   33 +++++---
 src/angle/kneeAngleGlobal.cpp    |    9 ++
 src/angle/kneeAngleRInside.cpp   |  150 ++++++++++++++++++++++++++++++++++++++
 src/angle/kneeAngleUtil.cpp      |   10 +-
 5 files changed, 267 insertions(+), 33 deletions(-)
---
diff --git a/src/angle/kneeAngle.cpp b/src/angle/kneeAngle.cpp
index d7fa7fa..0027835 100644
--- a/src/angle/kneeAngle.cpp
+++ b/src/angle/kneeAngle.cpp
@@ -138,19 +138,23 @@
 
 #include "opencv/cv.h"
 #include "opencv/highgui.h"
-#include <opencv/cxcore.h>
+//#include <opencv/cxcore.h>
 #include "stdio.h"
 #include "math.h"
-#include<iostream>
-#include<fstream>
-#include<vector>
+//#include<iostream>
+//#include<fstream>
+//#include<vector>
 #include <string>
 
+#include <RInside.h>
+
 #include "kneeAngleGlobal.cpp"
 #include "kneeAngleUtil.cpp"
 #include "kneeAngleFunctions.cpp"
+#include "kneeAngleSystemCalls.cpp"
+
 
-using namespace std;
+//using namespace std;
 
 int menu(IplImage *, CvFont);
 
@@ -162,7 +166,7 @@ int main(int argc,char **argv)
 		char *startMessage = new char[300];
 		sprintf(startMessage, "\nkneeAngle HELP.\n\nProvide file location as a first argument...\nOptional: as 2nd argument provide a fraction of video to start at that frame, or a concrete frame.\nOptional: as 3rd argument provide mode you want to execute (avoiding main menu).\n\t%d: validation; %d: blackWithoutMarkers; %d: skinOnlyMarkers; %d: blackOnlyMarkers.\n\nEg: Start at frame 5375:\n\tkneeAngle myfile.mov 5375\nEg:start at 80 percent of video and directly as blackOnlyMarkers:\n\tkneeAngle myFile.mov .8 %d\n", 
 				validation, blackWithoutMarkers, skinOnlyMarkers, blackOnlyMarkers, blackOnlyMarkers);
-		cout<< startMessage <<endl;
+		std::cout<< startMessage <<std::endl;
 		exit(1);
 	}
 
@@ -246,7 +250,8 @@ int main(int argc,char **argv)
 
 	double knee2Hip,knee2Toe,hip2Toe;
 	double thetaExpected, thetaMarked;
-	string text,angle;
+	//string text,angle;
+	std::string text;
 	double minThetaExpected = 360;
 	double minThetaMarked = 360;
 	double minThetaRealFlex = 360;
@@ -259,12 +264,16 @@ int main(int argc,char **argv)
 	FILE *fdatapre; //each line: 'current box height; current angle'
 	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
+
 	char header[] = "_header.txt";
 	char txt[] = ".txt";
 	char csv[] = ".csv";
 	char fheaderName [strlen(fileName) + strlen(header)];
 	char fdatapreName [strlen(fileName) + strlen(txt)];
 	char fdatapostName [strlen(fileName) + strlen(csv)];
+//	char fpointsdumpName[] = "pointsDump.csv";
 
 	if(programMode == validation) {
 //		cvNamedWindow("Holes_on_contour",1);
@@ -306,9 +315,19 @@ 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");
+		fclose(fpointsdump);
+	}
+	*/
+		
 	int kneePointWidth = -1;
 	int toePointWidth = -1;
 		
@@ -365,7 +384,13 @@ int main(int argc,char **argv)
 	CvPoint hipOldWorked = pointToZero();
 	CvPoint kneeOldWorked = pointToZero();
 	CvPoint toeOldWorked = pointToZero();
+	
+	CvSeq* seqPredicted;
+	CvPoint hipPredicted = pointToZero();
+	CvPoint kneePredicted = pointToZero();
+	CvPoint toePredicted = pointToZero();
 
+	
 	/*
 	int upLegMarkedDist = 0;
 	int upLegMarkedDistMax = 0;
@@ -469,6 +494,7 @@ int main(int argc,char **argv)
 
 	while(!shouldEnd) 
 	{
+
 		/*
 		 * 1
 		 * GET FRAME AND FLOW CONTROL
@@ -584,15 +610,28 @@ int main(int argc,char **argv)
 		 * FIND THREE MARKER POINTS
 		 */
 
+		
+
+		//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(programMode == skinOnlyMarkers || programMode == blackOnlyMarkers || programMode == validation) 
 		{
 
 /* kalman */
+			/*
 			const CvMat* prediction = cvKalmanPredict(kalman, 0);
 			CvPoint prediction_pt = cvPoint(
 					cvRound(prediction->data.fl[0]), 
 					cvRound(prediction->data.fl[1]));
+					*/
 /* /kalman */
 	
 
@@ -622,7 +661,7 @@ int main(int argc,char **argv)
 			CvSeq* seqHolesEnd;
 
 			if(programMode == skinOnlyMarkers) {
-				seqHolesEnd = findHolesSkin(output, frame_copy, hipMarked, kneeMarked, toeMarked, font);
+				seqHolesEnd = findHolesSkin(output, frame_copy, hipMarked, kneeMarked, toeMarked, hipPredicted, kneePredicted, toePredicted, font);
 			}
 			else { //if(programMode == blackOnlyMarkers || programMode == validation) 
 				//this segmented is to find the contour (threshold is lot little)
@@ -640,7 +679,7 @@ int main(int argc,char **argv)
 				cvCopy(frame_copy,frame_copyTemp);
 				seqHolesEnd = findHoles(
 						outputTemp, segmented, foundHoles, frame_copy,  
-						maxrect, hipOld, kneeOld, toeOld, font);
+						maxrect, hipOld, kneeOld, toeOld, hipPredicted, kneePredicted, toePredicted, font);
 
 				//if hip or toe is touching a border of the image
 				//then will not be included in largest contour
@@ -673,7 +712,7 @@ imageGuiResult(gui, "going", font);
 //printf("threshold :%d\n", threshold);
 //printf("thresholdLC :%d\n", thresholdLargestContour);
 //cvWaitKey(500); //to allow messages be shown
-					seqHolesEnd = findHolesSkin(output, frame_copy, hipMarked, kneeMarked, toeMarked, font);
+					seqHolesEnd = findHolesSkin(output, frame_copy, hipMarked, kneeMarked, toeMarked, hipPredicted, kneePredicted, toePredicted, font);
 imageGuiResult(gui, "returned", font);
 //cvWaitKey(500); //to allow gui image be shown
 				}
@@ -684,7 +723,29 @@ imageGuiResult(gui, "returned", font);
 			hipMarked = *CV_GET_SEQ_ELEM( CvPoint, seqHolesEnd, 0); 
 			kneeMarked = *CV_GET_SEQ_ELEM( CvPoint, seqHolesEnd, 1 ); 
 			toeMarked = *CV_GET_SEQ_ELEM( CvPoint, seqHolesEnd, 2 ); 
-			
+		
+
+			//if all the points are ok, the dump in pointsDump file to smooth and predict
+			if( ! pointIsNull(hipMarked) && ! pointIsNull(kneeMarked) && ! pointIsNull(toeMarked) ) {
+				/*
+				if((fpointsdump=fopen(fpointsdumpName,"a"))==NULL){
+					printf("Error, no se puede añadir en el fichero %s\n",fpointsdumpName);
+				} else {
+					fprintf(fpointsdump, "%d;%d;%d;%d;%d;%d\n", hipMarked.x, hipMarked.y, 
+							kneeMarked.x, kneeMarked.y, toeMarked.x, toeMarked.y);
+				}
+				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);
+			}
+
+
 			
 
 // kalman 
@@ -1638,10 +1699,17 @@ imageGuiResult(gui, "returned", font);
 					cvThreshold(gray,segmented,thresholdLargestContour,thresholdMax,CV_THRESH_BINARY_INV);
 
 					maxrect = findLargestContour(segmented, outputTemp, showContour);
-					//frame_copyTemp = cvCreateImage( cvSize(frame->width,frame->height),IPL_DEPTH_8U, frame->nChannels );
+					
+					//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); 
+
+					
 					findHoles(
 							outputTemp, segmented, foundHoles, frame_copyTemp,  
-							maxrect, hipOld, kneeOld, toeOld, font);
+							maxrect, hipOld, kneeOld, toeOld, hipPredicted, kneePredicted, toePredicted, font);
 
 					cvCopy(segmentedValidationHoles, output);
 					cvShowImage("threshold", output);
diff --git a/src/angle/kneeAngleFunctions.cpp b/src/angle/kneeAngleFunctions.cpp
index a831996..1de7b6f 100644
--- a/src/angle/kneeAngleFunctions.cpp
+++ b/src/angle/kneeAngleFunctions.cpp
@@ -24,12 +24,12 @@
 
 #include "opencv/cv.h"
 #include "opencv/highgui.h"
-#include <opencv/cxcore.h>
+//#include <opencv/cxcore.h>
 #include "stdio.h"
 #include "math.h"
-#include<iostream>
-#include<fstream>
-#include<vector>
+//#include<iostream>
+//#include<fstream>
+//#include<vector>
 #include <string>
 
 
@@ -345,7 +345,10 @@ CvPoint findToePoint(IplImage *img,CvRect roirect,int startx,int starty, int toe
  * imgH (image Holes)
  */
 CvSeq* findHoles(IplImage *imgC, IplImage *imgH, IplImage *foundHoles, IplImage *imgMain, 
-	CvRect roirect, CvPoint hipOld, CvPoint kneeOld, CvPoint toeOld, CvFont font)
+	CvRect roirect, 
+	CvPoint hipOld, CvPoint kneeOld, CvPoint toeOld, 
+	CvPoint hipPredicted, CvPoint kneePredicted, CvPoint toePredicted, 
+	CvFont font)
 {
 	CvPoint pt;
 	pt.x =0;pt.y=0;
@@ -518,16 +521,17 @@ CvSeq* findHoles(IplImage *imgC, IplImage *imgH, IplImage *foundHoles, IplImage
 				if(size == sizeBig1 || size == sizeBig2 || size == sizeBig3)
 					validSure = true;
 			} 
-			//if found a point before, and this point is inside before point (ok at 300 fps)
+			//if found a point before, and this point is where it's predicted, or inside the before point (ok at 300 fps)
 			//a point is also ok, if we come from a user forward (then, there's not need to be inside old point)
-			else if (pointInside(hipOld, sp1, sp2) || pointInside(kneeOld, sp1,sp2) || pointInside(toeOld, sp1,sp2))
+			else if (pointInside(hipPredicted, sp1, sp2) || pointInside(kneePredicted, sp1,sp2) || pointInside(toePredicted, sp1,sp2)
+					|| (pointInside(hipOld, sp1, sp2) || pointInside(kneeOld, sp1,sp2) || pointInside(toeOld, sp1,sp2)))
 				validSure = true;
 		}
 
 		if(validSure) {
 			CvPoint center = *CV_GET_SEQ_ELEM( CvPoint, seqHolesCenter, i ); 
 
-			//the point will be hip knee or toe depending on the distance betwee old hipe, knee & toe
+			//the point will be hip knee or toe depending on the distance betwee old hip, knee & toe
 			//but give preference to the top (<=) because will be the first to be found (top to bottom)
 			int pointIs = TOGGLENOTHING;
 			if(!pointIsNull(hipOld) && !pointIsNull(kneeOld) && !pointIsNull(toeOld)) {
@@ -635,7 +639,10 @@ CvSeq* findHoles(IplImage *imgC, IplImage *imgH, IplImage *foundHoles, IplImage
  * this function is realy similiar to findHoles
  * try to do only a function
  */
-CvSeq* findHolesSkin(IplImage *imgThresh, IplImage *imgColor, CvPoint hipOld, CvPoint kneeOld, CvPoint toeOld, CvFont font)
+CvSeq* findHolesSkin(IplImage *imgThresh, IplImage *imgColor, 
+		CvPoint hipOld, CvPoint kneeOld, CvPoint toeOld, 
+		CvPoint hipPredicted, CvPoint kneePredicted, CvPoint toePredicted, 
+		CvFont font)
 {
 	CvPoint pt;
 	pt.x =0;pt.y=0;
@@ -827,23 +834,23 @@ CvSeq* findHolesSkin(IplImage *imgThresh, IplImage *imgColor, CvPoint hipOld, Cv
 					}
 				}
 			}
-			//if found a point before, and this point is inside before point (ok at 300 fps)
+			//if found a point before, and this point is where it's predicted, or inside the before point (ok at 300 fps)
 			//a point is also ok, if we come from a user forward (then, there's not need to be inside old point)
 			else {
 				validSure = true;
-				if(pointInside(hipOld, sp1, sp2)) {
+				if(pointInside(hipPredicted, sp1, sp2) || pointInside(hipOld, sp1, sp2)) {
 					hipPoint.x = center.x; 
 					hipPoint.y = center.y;
 					color = RED;
 					sprintf(labelShort,"H");
 				} 
-				else if(pointInside(kneeOld, sp1,sp2)) {
+				else if(pointInside(kneePredicted, sp1, sp2) || pointInside(kneeOld, sp1, sp2)) {
 					kneePoint.x = center.x; 
 					kneePoint.y = center.y;
 					color = GREEN;
 					sprintf(labelShort,"K");
 				} 
-				else if(pointInside(toeOld, sp1,sp2)) {
+				else if(pointInside(toePredicted, sp1, sp2) || pointInside(toeOld, sp1, sp2)) {
 					toePoint.x = center.x; 
 					toePoint.y = center.y;
 					color = BLUE;
diff --git a/src/angle/kneeAngleGlobal.cpp b/src/angle/kneeAngleGlobal.cpp
index 12073e6..4f3bed6 100644
--- a/src/angle/kneeAngleGlobal.cpp
+++ b/src/angle/kneeAngleGlobal.cpp
@@ -114,4 +114,13 @@ bool mouseMultiplier = false; //using shift key
 bool zoomed = false;
 double zoomScale = 2; 
 
+//predictions stuff
+RInside R = RInside();              // create an embedded R instance 
+	
+std::vector<int> hipXVector;
+std::vector<int> hipYVector;
+std::vector<int> kneeXVector;
+std::vector<int> kneeYVector;
+std::vector<int> toeXVector;
+std::vector<int> toeYVector;
 
diff --git a/src/angle/kneeAngleRInside.cpp b/src/angle/kneeAngleRInside.cpp
new file mode 100644
index 0000000..1062818
--- /dev/null
+++ b/src/angle/kneeAngleRInside.cpp
@@ -0,0 +1,150 @@
+/*
+ * This file is part of ChronoJump
+ *
+ * Chronojump is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *   the Free Software Foundation; either version 2 of the License, or   
+ *    (at your option) any later version.
+ *    
+ * Chronojump is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 
+ *    GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+# Copyright (C) 2010   Xavier de Blas <xaviblas gmail com> , Carlos J. Gil Bellosta <cgb datanalytics com>
+ *
+ */
+
+
+#include "stdio.h"
+#include <sys/wait.h>
+#include <string>
+
+
+CvSeq* predictPointsRInside()
+{
+	/* TESTING:
+
+	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);
+	*/
+
+	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);
+	}
+	
+	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 );
+	cvSeqPush( seqPoints, &toe );
+	
+	return seqPoints;
+}
+
diff --git a/src/angle/kneeAngleUtil.cpp b/src/angle/kneeAngleUtil.cpp
index 03e2b35..f06d597 100644
--- a/src/angle/kneeAngleUtil.cpp
+++ b/src/angle/kneeAngleUtil.cpp
@@ -23,14 +23,14 @@
 
 #include "opencv/cv.h"
 #include "opencv/highgui.h"
-#include <opencv/cxcore.h>
+//#include <opencv/cxcore.h>
 #include "stdio.h"
 #include "math.h"
-#include<iostream>
-#include<fstream>
-#include<vector>
+//#include<iostream>
+//#include<fstream>
+//#include<vector>
 #include <string>
-using namespace std;
+//using namespace std;
 
 
 CvPoint findMiddle(CvPoint pt1, CvPoint pt2)
[Date Prev][Date Next]   [Thread Prev][Thread Next]   [Thread Index] [Date Index] [Author Index]