First CommitHEADmaster

1 files changed, 256 insertions, 0 deletions

diff --git a/sci_gateway1/cpp/opencv_projectPoints.cpp b/sci_gateway1/cpp/opencv_projectPoints.cpp
new file mode 100644
index 0000000..d242875
--- /dev/null
+++ b/sci_gateway1/cpp/opencv_projectPoints.cpp
@@ -0,0 +1,256 @@
+#include <numeric>
+#include "opencv2/core/core.hpp"
+#include "opencv2/highgui/highgui.hpp"
+#include "opencv2/opencv.hpp"
+#include "opencv2/imgproc/imgproc.hpp"
+#include "opencv2/calib3d/calib3d.hpp"
+#include <iostream>
+using namespace cv;
+using namespace std;
+extern "C"
+{
+  #include "api_scilab.h"
+  #include "Scierror.h"
+  #include "BOOL.h"
+  #include <localization.h>
+  #include <sciprint.h>
+  #include "../common.h"
+  
+  int opencv_projectPoints(char *fname, unsigned long fname_len)
+  {
+  
+   SciErr sciErr;
+   int intErr=0;
+   int iComplex = 0;
+   int iType = 0;
+   int *piAddr  = NULL;
+   int i,size,j;
+   double *worldPoints=NULL,*rVector=NULL,*tVector=NULL,*intrinsicMatrix=NULL,*distCoefficients=NULL;
+   int wpRows,wpCols,rvRows,rvCols,tvRows,tvCols,imRows,imCols,dcRows,dcCols;
+   double *points=NULL;
+  
+    vector<cv::Point3d> objectPoints;
+    vector<cv::Point2d> imagePoints;
+    Mat intrisicMat(3, 3, cv::DataType<double>::type);
+    Mat rVec(3, 1, cv::DataType<double>::type);
+    Mat tVec(3, 1, cv::DataType<double>::type);
+    Mat distCoeffs(5, 1, cv::DataType<double>::type);
+  
+  	CheckInputArgument(pvApiCtx, 5, 5);
+   CheckOutputArgument(pvApiCtx, 1, 1) ;
+  
+  	sciErr = getVarAddressFromPosition(pvApiCtx, 1, &piAddr);
+	if(sciErr.iErr)
+	{
+		printError(&sciErr, 0);
+		return 0;
+	}
+	sciErr = getVarType(pvApiCtx, piAddr, &iType);
+	if(sciErr.iErr || iType != sci_matrix)
+	{
+		printError(&sciErr, 0);
+		return 0;
+	}
+	//Get complexity
+	iComplex = isVarComplex(pvApiCtx, piAddr);
+	//Check complexity
+	if(iComplex)
+	{
+		Scierror(999, "%s: Wrong type for input argument: A complex number is not expected.\n");
+		return 0;
+	}	
+ 	sciErr = getMatrixOfDouble(pvApiCtx, piAddr, &wpRows, &wpCols, &worldPoints);
+ 	if(sciErr.iErr)
+	{
+		printError(&sciErr, 0);
+		return 0;
+	}
+  	if(wpCols!=2)
+  	{
+		Scierror(999, "%s: Wrong value for input argument: World points must be M*2\n");
+		return 0;
+  	}
+  	
+  	sciErr = getVarAddressFromPosition(pvApiCtx, 2, &piAddr);
+	if(sciErr.iErr)
+	{
+		printError(&sciErr, 0);
+		return 0;
+	}
+	sciErr = getVarType(pvApiCtx, piAddr, &iType);
+	if(sciErr.iErr || iType != sci_matrix)
+	{
+		printError(&sciErr, 0);
+		return 0;
+	}
+	//Get complexity
+	iComplex = isVarComplex(pvApiCtx, piAddr);
+	//Check complexity
+	if(iComplex)
+	{
+		Scierror(999, "%s: Wrong type for input argument: A complex number is not expected.\n");
+		return 0;
+	}	
+ 	sciErr = getMatrixOfDouble(pvApiCtx, piAddr, &rvRows, &rvCols, &rVector);
+ 	if(sciErr.iErr)
+	{
+		printError(&sciErr, 0);
+		return 0;
+	}
+	if(rvCols!=3)
+	{
+		
+	}
+	sciErr = getVarAddressFromPosition(pvApiCtx, 3, &piAddr);
+	if(sciErr.iErr)
+	{
+		printError(&sciErr, 0);
+		return 0;
+	}
+	sciErr = getVarType(pvApiCtx, piAddr, &iType);
+	if(sciErr.iErr || iType != sci_matrix)
+	{
+		printError(&sciErr, 0);
+		return 0;
+	}
+	//Get complexity
+	iComplex = isVarComplex(pvApiCtx, piAddr);
+	//Check complexity
+	if(iComplex)
+	{
+		Scierror(999, "%s: Wrong type for input argument: A complex number is not expected.\n");
+		return 0;
+	}	
+ 	sciErr = getMatrixOfDouble(pvApiCtx, piAddr, &tvRows, &tvCols, &tVector);
+ 	if(sciErr.iErr)
+	{
+		printError(&sciErr, 0);
+		return 0;
+	}
+	if(tvCols!=3)
+	{
+		
+	}
+	if(rvRows!=tvRows)
+	{
+		
+	}
+	
+	sciErr = getVarAddressFromPosition(pvApiCtx, 4, &piAddr);
+	if(sciErr.iErr)
+	{
+		printError(&sciErr, 0);
+		return 0;
+	}
+	sciErr = getVarType(pvApiCtx, piAddr, &iType);
+	if(sciErr.iErr || iType != sci_matrix)
+	{
+		printError(&sciErr, 0);
+		return 0;
+	}
+	//Get complexity
+	iComplex = isVarComplex(pvApiCtx, piAddr);
+	//Check complexity
+	if(iComplex)
+	{
+		Scierror(999, "%s: Wrong type for input argument: A complex number is not expected.\n");
+		return 0;
+	}	
+ 	sciErr = getMatrixOfDouble(pvApiCtx, piAddr, &imRows, &imCols, &intrinsicMatrix);
+ 	if(sciErr.iErr)
+	{
+		printError(&sciErr, 0);
+		return 0;
+	}
+	if(imRows!=3 || imCols!=3)
+	{
+		
+	}
+	
+	
+	sciErr = getVarAddressFromPosition(pvApiCtx, 5, &piAddr);
+	if(sciErr.iErr)
+	{
+		printError(&sciErr, 0);
+		return 0;
+	}
+	sciErr = getVarType(pvApiCtx, piAddr, &iType);
+	if(sciErr.iErr || iType != sci_matrix)
+	{
+		printError(&sciErr, 0);
+		return 0;
+	}
+	//Get complexity
+	iComplex = isVarComplex(pvApiCtx, piAddr);
+	//Check complexity
+	if(iComplex)
+	{
+		Scierror(999, "%s: Wrong type for input argument: A complex number is not expected.\n");
+		return 0;
+	}	
+ 	sciErr = getMatrixOfDouble(pvApiCtx, piAddr, &dcRows, &dcCols, &distCoefficients);
+ 	if(sciErr.iErr)
+	{
+		printError(&sciErr, 0);
+		return 0;
+	}
+	if(dcRows!=1 || dcCols!=3)
+	{
+		
+	}
+	
+	for(i=0;i<wpRows;++i)
+	{
+		objectPoints.push_back(cv::Point3d(worldPoints[i],worldPoints[i+wpRows],0));
+	}
+	
+	intrisicMat.at<double>(0, 0) = intrinsicMatrix[0];
+    intrisicMat.at<double>(1, 0) = 0;
+    intrisicMat.at<double>(2, 0) = 0;
+    
+    intrisicMat.at<double>(0, 1) = 0;
+    intrisicMat.at<double>(1, 1) = intrinsicMatrix[4];
+    intrisicMat.at<double>(2, 1) = 0;
+
+    intrisicMat.at<double>(0, 2) = intrinsicMatrix[2];
+    intrisicMat.at<double>(1, 2) = intrinsicMatrix[5];
+    intrisicMat.at<double>(2, 2) = 1;
+
+	rVec.at<double>(0) = rVector[0];
+    rVec.at<double>(1) = rVector[1];
+    rVec.at<double>(2) = rVector[2];
+
+    tVec.at<double>(0) = tVector[0];
+    tVec.at<double>(1) = tVector[1];
+    tVec.at<double>(2) = tVector[2];
+    
+    distCoeffs.at<double>(0) = distCoefficients[0];
+    distCoeffs.at<double>(1) = distCoefficients[1];
+    distCoeffs.at<double>(2) = distCoefficients[2];
+    distCoeffs.at<double>(3) = distCoefficients[3];
+    if(dcCols==5)
+    {
+    	distCoeffs.at<double>(4) = distCoefficients[4];
+    }
+
+	//Actual process
+    projectPoints(objectPoints, rVec, tVec, intrisicMat, distCoeffs, imagePoints);
+    size=imagePoints.size();
+    points = (double*) malloc(sizeof(double) * size * 2);
+    for(i = 0 ; i < size ; i++)
+	{
+		points[i] 	 = imagePoints[i].x;
+		points[i+size] = imagePoints[i].y;
+	}
+	
+	sciErr = createMatrixOfDouble(pvApiCtx, nbInputArgument(pvApiCtx) + 1, size , 2, points);
+	if(sciErr.iErr)
+	{
+		printError(&sciErr, 0);
+		return 0;
+	}
+	AssignOutputVariable(pvApiCtx, 1) = nbInputArgument(pvApiCtx) + 1;
+    ReturnArguments(pvApiCtx);
+    return 0;
+  }  
+}