First CommitHEADmaster

1 files changed, 318 insertions, 0 deletions

diff --git a/sci_gateway1/cpp/opencv_estimateFundamentalMatrix.cpp b/sci_gateway1/cpp/opencv_estimateFundamentalMatrix.cpp
new file mode 100644
index 0000000..4a2cec2
--- /dev/null
+++ b/sci_gateway1/cpp/opencv_estimateFundamentalMatrix.cpp
@@ -0,0 +1,318 @@
+/****************************************************************************************************
+estimateFundamentalMatrix(InputArray1,InputArray2,'method','RANSAC','param1',2,'param2',0.99);
+
+possible methods are 7POINT,8POINT,RANSAC,LMEDS
+param1- It indiacates maximum distance from a point to an epipolar line in pixels
+param2- It specifies a desirable level of confidence
+*********************************************************	***********************************************/
+#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_estimateFundamentalMat(char *fname, unsigned long fname_len)
+  {
+  
+   SciErr sciErr;
+   int intErr=0;
+   int iComplex = 0;
+   int iType = 0;
+   int *piAddr  = NULL;
+   int *piLen = NULL;
+   int iRows,iCols;
+   int iA1Rows=0,iA1Cols=0,iA2Rows=0,iA2Cols=0;
+   int method=FM_RANSAC;
+   int nbInputArguments,iter,i,j;
+   char **pstData = NULL; 
+   char *currentArg = NULL; 
+   bool *providedArgs = NULL; 
+   double* pdblReal	= NULL;
+   double *iArray1=NULL,*iArray2=NULL;
+   double param1=3,param2=0.99;
+
+   vector<Point2f> point1,point2;
+  
+  CheckInputArgument(pvApiCtx, 2, 8);
+  CheckOutputArgument(pvApiCtx, 1, 1) ;
+  	
+  nbInputArguments = *getNbInputArgument(pvApiCtx); 
+  providedArgs = (bool*) malloc(sizeof(bool) * 3); 
+  
+  
+    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, &iA1Rows, &iA1Cols, &iArray1);
+ 	if(sciErr.iErr)
+	{
+		printError(&sciErr, 0);
+		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, &iA2Rows, &iA2Cols, &iArray2);
+ 	if(sciErr.iErr)
+	{
+		printError(&sciErr, 0);
+		return 0;
+	}
+	if(iA1Rows != iA2Rows)
+    {
+    	Scierror(999, "There must be the same number of points in both input arrays\n"); 
+        return 0; 
+    }
+    
+	for(i=0;i<iA1Rows;i++){
+		point1.push_back(Point2f(iArray1[i],iArray1[iA1Rows+i]));
+		point2.push_back(Point2f(iArray2[i],iArray2[iA2Rows+i]));
+	}
+	for(iter=3; iter<= nbInputArguments; iter++)
+    {
+		sciErr = getVarAddressFromPosition(pvApiCtx, iter, &piAddr); 
+        if (sciErr.iErr)
+        {
+        	printError(&sciErr, 0); 
+            return 0; 
+        }
+        sciErr = getMatrixOfString(pvApiCtx, piAddr, &iRows, &iCols, NULL, NULL); 
+            if (sciErr.iErr)
+            {
+                printError(&sciErr, 0); 
+                return 0; 
+            }
+            
+            piLen = (int*) malloc(sizeof(int) * iRows * iCols); 
+            
+            sciErr = getMatrixOfString(pvApiCtx,  piAddr,  &iRows,  &iCols,  piLen,  NULL); 
+            if (sciErr.iErr)
+            {
+                printError(&sciErr, 0); 
+                return 0; 
+            }
+            
+            pstData = (char**) malloc(sizeof(char*) * iRows * iCols); 
+            for(int iterPstData = 0; iterPstData < iRows * iCols; iterPstData++)
+            {
+                pstData[iterPstData] = (char*) malloc(sizeof(char) * piLen[iterPstData] + 1); 
+            }
+            
+            sciErr = getMatrixOfString(pvApiCtx, piAddr, &iRows, &iCols, piLen, pstData); 
+            if (sciErr.iErr)
+            {
+                printError(&sciErr, 0); 
+                return 0; 
+            }
+            
+            currentArg = pstData[0]; 
+            if(strcmp(currentArg, "method")==0)
+            {
+				//sciprint("in if\tcurrentArg:%s\n",currentArg);            
+                if(iter+1<=nbInputArguments && !providedArgs[0])
+                {
+                    sciErr = getVarAddressFromPosition(pvApiCtx, ++iter, &piAddr); 
+                    if (sciErr.iErr)
+                    {
+                        printError(&sciErr, 0); 
+                        return 0; 
+                    }
+                    sciErr = getMatrixOfString(pvApiCtx, piAddr, &iRows, &iCols, NULL, NULL); 
+				    if (sciErr.iErr)
+				    {
+				        printError(&sciErr, 0); 
+				        return 0; 
+				    }
+				    
+				    piLen = (int*) malloc(sizeof(int) * iRows * iCols); 
+				    
+				    sciErr = getMatrixOfString(pvApiCtx,  piAddr,  &iRows,  &iCols,  piLen,  NULL); 
+				    if (sciErr.iErr)
+				    {
+				        printError(&sciErr, 0); 
+				        return 0; 
+				    }
+				    
+				    pstData = (char**) malloc(sizeof(char*) * iRows * iCols); 
+				    for(int iterPstData = 0; iterPstData < iRows * iCols; iterPstData++)
+				    {
+				        pstData[iterPstData] = (char*) malloc(sizeof(char) * piLen[iterPstData] + 1); 
+				    }
+				    
+				    sciErr = getMatrixOfString(pvApiCtx, piAddr, &iRows, &iCols, piLen, pstData); 
+				    if (sciErr.iErr)
+				    {
+				        printError(&sciErr, 0); 
+				        return 0; 
+				    }
+				    currentArg=pstData[0];
+                    // Checking if values are in proper range. Same for all optional arguments
+                    //sciprint("in if\tcurrentArg:%s\n",currentArg);
+                    if(strcmp(currentArg,"7POINT")==0)
+                    	 method = FM_7POINT;	  
+					else if (strcmp(currentArg,"8POINT")==0)
+						  method= FM_8POINT;
+					else if (strcmp(currentArg,"RANSAC")==0)
+						  method= FM_RANSAC;
+					else if (strcmp(currentArg,"LMEDS")==0)
+						  method= FM_LMEDS;
+					else
+					{
+						sciprint("Invalid method given. RANSAC was used instead");
+						method= FM_RANSAC;
+					}	  
+                    providedArgs[0] = 1; 
+                }
+                else if(providedArgs[0]) // Send an error message if an argument is provided more than once. Same for all optional arguments.
+                {
+                    Scierror(999, "Please provide optional arguments only once.\n"); 
+                    return 0; 
+                }
+                else // Send an error message if name of argument is given but type is incorrect. Same for all optional arguments.
+                {
+                    Scierror(999, "Incorrect number of arguments provided. Please check the documentation for more information.\n"); 
+                    return 0; 
+                }
+            }    
+            else if(strcmp(currentArg, "param1")==0)
+            {
+                if(iter+1 <= nbInputArguments  && !providedArgs[1])
+                {
+                    sciErr = getVarAddressFromPosition(pvApiCtx, ++iter, &piAddr); 
+                    if (sciErr.iErr)
+                    {
+                        printError(&sciErr, 0); 
+                        return 0; 
+                    }
+                    intErr = getScalarDouble(pvApiCtx, piAddr, &param1); 
+                    if(intErr)
+                    {
+                        return intErr; 
+                    }
+                    providedArgs[1] = 1; 
+                }
+                else if(providedArgs[1])
+                {
+                    Scierror(999, "Please provide optional arguments only once.\n"); 
+                    return 0; 
+                }
+                else
+                {
+                    Scierror(999, "Incorrect number of arguments provided. Please check the documentation for more information.\n"); 
+                    return 0; 
+                }
+            }
+            else if(strcmp(currentArg, "param2")==0)
+            {
+                if(iter+1 <= nbInputArguments  && !providedArgs[2])
+                {
+                    sciErr = getVarAddressFromPosition(pvApiCtx, ++iter, &piAddr); 
+                    if (sciErr.iErr)
+                    {
+                        printError(&sciErr, 0); 
+                        return 0; 
+                    }
+                    intErr = getScalarDouble(pvApiCtx, piAddr, &param2); 
+                    if(intErr)
+                    {
+                        return intErr; 
+                    }
+                    providedArgs[2] = 1; 
+                }
+                else if(providedArgs[2])
+                {
+                    Scierror(999, "Please provide optional arguments only once.\n"); 
+                    return 0; 
+                }
+                else
+                {
+                    Scierror(999, "Incorrect number of arguments provided. Please check the documentation for more information.\n"); 
+                    return 0; 
+                }
+            }
+            else
+            {
+                Scierror(999, "Error: \"%s\" name for input argument is not valid.\n", currentArg); 
+                return 0; 
+            }
+	}
+	
+	if (method==FM_7POINT && point1.size()!=7){
+		Scierror(999, "Error: For FM_7POINT method number of points in each array must be 7\n"); 
+        return 0;
+	}
+	if (method!=FM_7POINT && point1.size()<8 ){
+		Scierror(999, "Error: Number of points in each array must be greater than or equal to 8\n");
+		return 0;
+	}
+	
+	Mat fm=cv::findFundamentalMat(point1, point2, method, param1, param2);
+	pdblReal = (double*) malloc(sizeof(double) * fm.rows *3); 
+	for(i = 0 ; i < fm.rows ; i++)
+	{
+		for(j = 0 ; j < 3 ; j++)
+		{
+			pdblReal[i + fm.rows * j] = fm.at<double>(i,j);
+		}
+	}
+	sciErr = createMatrixOfDouble(pvApiCtx, nbInputArgument(pvApiCtx) + 1, fm.rows , 3, pdblReal);
+	if(sciErr.iErr)
+	{
+		printError(&sciErr, 0);
+		return 0;
+	}
+	AssignOutputVariable(pvApiCtx, 1) = nbInputArgument(pvApiCtx) + 1;
+    ReturnArguments(pvApiCtx);
+    return 0;
+  }  
+}