diff options
Diffstat (limited to 'sci_gateway1/cpp/opencv_undistortPoints.cpp')
| -rw-r--r-- | sci_gateway1/cpp/opencv_undistortPoints.cpp | 190 |
1 files changed, 190 insertions, 0 deletions
diff --git a/sci_gateway1/cpp/opencv_undistortPoints.cpp b/sci_gateway1/cpp/opencv_undistortPoints.cpp new file mode 100644 index 0000000..dff80e4 --- /dev/null +++ b/sci_gateway1/cpp/opencv_undistortPoints.cpp @@ -0,0 +1,190 @@ +/*************************************************** +Author : Deepshikha +***************************************************/ + +#include <numeric> +#include "opencv2/core/core.hpp" +#include "opencv2/highgui/highgui.hpp" +#include "opencv2/opencv.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_undistortPoints(char *fname, unsigned long fname_len) + { + //variables + int i,j,k,n,m; + int iRows=0,iCols=0; + int *piAddr1 = NULL; + int *piAddr2 = NULL; + int *piAddr3 = NULL; + int *piAddr4 = NULL; + int *piAddr5 = NULL; + + double *pdblReal = NULL; + + SciErr sciErr; + + //function varible + std::vector< Point2d > Src; + std::vector< Point2d > dst; + double cameraMatrix [3][3]; + double newCameraMatrix [3][3]; + double *distCoeffs = NULL; + double RectificationMatrix[3][3]; + + /*------------------------------------------- Checking Input Argument ----------------------------------------------------*/ + + CheckInputArgument(pvApiCtx, 5, 5); + CheckOutputArgument(pvApiCtx, 1, 1) ; + + /*------------------------------------- Source matrix :- First argument of the input -------------------------------------*/ + + sciErr = getVarAddressFromPosition(pvApiCtx, 1, &piAddr1); + if (sciErr.iErr) + { + printError(&sciErr, 0); + return 0; + } + sciErr = getMatrixOfDouble(pvApiCtx, piAddr1, &iRows, &iCols, &pdblReal); + if(sciErr.iErr) + { + printError(&sciErr, 0); + return 0; + } + // For 1 X N single channel + for(i = 0; i < iRows; ++i) + Src.push_back( Point2d(pdblReal[(0 * iRows) + i], pdblReal[(1 * iRows) + i])); + + /* ------------------------------------ Camera matrix :- Second argument of the input -------------------------------------*/ + + sciErr = getVarAddressFromPosition(pvApiCtx, 2, &piAddr2); + if (sciErr.iErr) + { + printError(&sciErr, 0); + return 0; + } + sciErr = getMatrixOfDouble(pvApiCtx, piAddr2, &iRows, &iCols, &pdblReal); + if(sciErr.iErr) + { + printError(&sciErr, 0); + return 0; + } + Mat cameraMat(3, 3, CV_64F); + for(i=0;i<3;i++) + for(j=0;j<3;j++) + cameraMat.at<double>(i,j) = pdblReal[(j * 3) + i]; + + + /* ------------------------------------ Distortion Coefficient matrix :- Third argument of the input -------------------------------------*/ + + sciErr = getVarAddressFromPosition(pvApiCtx,3,&piAddr3); + if (sciErr.iErr) + { + printError(&sciErr, 0); + return 0; + } + sciErr = getMatrixOfDouble(pvApiCtx, piAddr3, &iRows, &iCols, &pdblReal); + if(sciErr.iErr) + { + printError(&sciErr, 0); + return 0; + } + + if(iRows== 1) + n = iCols; + else if (iCols == 1) + n = iRows; + else{ + Scierror(1,"Points matrix (arg 4) must be a 2 X N matrix"); + return 0; + } + if(n==4 or n==5 or n==8); + else{ + Scierror(1," n must be 4 or 5 or 8"); + return 0; + } + + Mat distCoeffsActual(1,n,CV_64F); + for(i=0;i<n;i++) + distCoeffsActual.at<double>(0,i) = pdblReal[i]; + + + /* ------------------------------------ Rectification matrix :- Fourth argument of the input -------------------------------------*/ + + sciErr = getVarAddressFromPosition(pvApiCtx, 4, &piAddr4); + if (sciErr.iErr) + { + printError(&sciErr, 0); + return 0; + } + sciErr = getMatrixOfDouble(pvApiCtx, piAddr4, &iRows, &iCols, &pdblReal); + if(sciErr.iErr) + { + printError(&sciErr, 0); + return 0; + } + Mat RectMat(3, 3, CV_64F); + for(i=0;i<3;i++) + for(j=0;j<3;j++) + RectMat.at<double>(i,j) =pdblReal[(j*3)+i]; + + + /* ------------------------------------ New Camera matrix :- Fifth argument of the input -------------------------------------*/ + + sciErr = getVarAddressFromPosition(pvApiCtx,5,&piAddr5); + if (sciErr.iErr) + { + printError(&sciErr, 0); + return 0; + } + sciErr = getMatrixOfDouble(pvApiCtx, piAddr5, &iRows, &iCols , &pdblReal); + if(sciErr.iErr) + { + printError(&sciErr, 0); + return 0; + } + + Mat newCameraMat(3,3,CV_64F); + for(i=0;i<3;i++) + for(j=0;j<3;j++) + newCameraMat.at<double>(i, j) =pdblReal[(j*3)+i]; + + + + /* opencv function for undistortPoints */ + undistortPoints(Src, dst, cameraMat, distCoeffsActual, RectMat, newCameraMat); + + /* Writing to Scilab */ + double *pstdata = NULL; + pstdata = (double*)malloc(sizeof(double) * dst.size() * 2); + + for(i = 0; i < dst.size(); i++) + { + pstdata[i + 0 * dst.size()] = dst[i].x; + pstdata[i + 1 * dst.size()] = dst[i].y; + } + + sciErr = createMatrixOfDouble(pvApiCtx, nbInputArgument(pvApiCtx) + 1, dst.size(), 2, pstdata); + if(sciErr.iErr){ + printError(&sciErr, 0); + return 0; + } + + //Assigning the list as the Output Variable + AssignOutputVariable(pvApiCtx, 1) = nbInputArgument(pvApiCtx) + 1; + //Returning the Output Variables as arguments to the Scilab environment + ReturnArguments(pvApiCtx); + return 0; + + } +/* ==================================================================== */ +}
\ No newline at end of file |