diff --git a/ChangeLog b/ChangeLog index c3de56b..88f8560 100644 --- a/ChangeLog +++ b/ChangeLog @@ -1,3 +1,17 @@ +VERSION 1.3 + - Compatibility with OpenCV 3 + - In Marker Detector: + - Paralelization using OpenMP + - Marked as deprecated enableErosion and pyrDown + - Added functionality for markers with "locked corners". We refer to marker whose corners are connected + either to another marker (formaing a chessboard pattern), or to another black square. In this mode, + the use of subcorner refinement methods is expected to be more precise. See enableLockedCornersMethod() + - Added funcionality to search for the first threshold parameter simultaneously in several + values. The process is parallelized in multiple threads. See setMultiThresholdSearch() + - In HRM markers: + - Speed up marker identification process + - Improve performance of dictionary generation process. + - Added LICENSE file VERSION 1.2.5 - New type of markers: highly reliable markers (hrm) including utils to use them @@ -57,4 +71,4 @@ VERSION 1.0.0 VERSION 0.9.5 Added support for Boards Added cmake support - Bugs fixed \ No newline at end of file + Bugs fixed diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..8aceb4c --- /dev/null +++ b/LICENSE @@ -0,0 +1,26 @@ + +Copyright 2011 Rafael Muñoz Salinas. All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are +permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this list of + conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, this list + of conditions and the following disclaimer in the documentation and/or other materials + provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND +FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR +CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +The views and conclusions contained in the software and documentation are those of the +authors and should not be interpreted as representing official policies, either expressed +or implied, of Rafael Muñoz Salinas. diff --git a/README b/README index 8e1e342..44ce799 100644 --- a/README +++ b/README @@ -80,7 +80,7 @@ The 'utils_hrm' folder contains some applications to test these markers \section COMPILING THE LIBRARY: - REQUIREMENTS: OpenCv >= 2.3.1, cmake >= 2.8 + REQUIREMENTS: OpenCv >= 2.4.9, cmake >= 2.8 OPTIONAL: OpenGL for aruco_test_gl and aruco_test_board_gl \subsection LINUX: diff --git a/dist.info b/dist.info index 3842583..7636f1d 100644 --- a/dist.info +++ b/dist.info @@ -1,7 +1,7 @@ --- This file is part of LuaDist project name = "aruco" -version = "1.2.5" +version = "1.3" desc = "Augmented Reality library from the University of Cordoba" author = "Ava group of the Univeristy of Cordoba(Spain)" diff --git a/generateDoc.cmake b/generateDoc.cmake index 296f784..38adfac 100644 --- a/generateDoc.cmake +++ b/generateDoc.cmake @@ -1,8 +1,8 @@ -# -helper macro to add a "doc" target with CMake build system. +# -helper macro to add a "doc" target with CMake build system. # and configure doxy.config.in to doxy.config # # target "doc" allows building the documentation with doxygen/dot on WIN32 and Linux -# Creates .chm windows help file if MS HTML help workshop +# Creates .chm windows help file if MS HTML help workshop # (available from http://msdn.microsoft.com/workshop/author/htmlhelp) # is installed with its DLLs in PATH. # @@ -11,7 +11,7 @@ # doxygen, dot, latex, dvips, makeindex, gswin32, etc. # must be in path. # -# Note about Visual Studio Projects: +# Note about Visual Studio Projects: # MSVS hast its own path environment which may differ from the shell. # See "Menu Tools/Options/Projects/VC++ Directories" in VS 7.1 # @@ -69,21 +69,21 @@ IF (DOXYGEN_FOUND) IF (NOT DVIPS_CONVERTER) MESSAGE(STATUS "dvips command DVIPS_CONVERTER not found but usually required.") ENDIF (NOT DVIPS_CONVERTER) - + # Check config file IF (EXISTS "${DOX_CONFIG_FILE}") CONFIGURE_FILE(${DOX_CONFIG_FILE} ${CMAKE_CURRENT_BINARY_DIR}/doxy.config @ONLY ) #OUT-OF-PLACE LOCATION SET(DOXY_CONFIG "${CMAKE_CURRENT_BINARY_DIR}/doxy.config") - ELSE () + ELSE () MESSAGE(SEND_ERROR "Please create configuration file for doxygen in ${CMAKE_CURRENT_SOURCE_DIR}") ENDIF() # Add target - ADD_CUSTOM_TARGET(doc ALL ${DOXYGEN_EXECUTABLE} ${DOXY_CONFIG}) + ADD_CUSTOM_TARGET(doc ${DOXYGEN_EXECUTABLE} ${DOXY_CONFIG}) IF (WIN32 AND GENERATE_WIN_CHM STREQUAL "YES") FIND_PACKAGE(HTMLHelp) - IF (HTML_HELP_COMPILER) + IF (HTML_HELP_COMPILER) ADD_CUSTOM_TARGET(winhelp ${HTML_HELP_COMPILER} ${HHP_FILE}) ADD_DEPENDENCIES (winhelp doc) IF (EXISTS ${CHM_FILE}) @@ -97,7 +97,7 @@ IF (DOXYGEN_FOUND) IF (${PROJECT_NAME}_VERSION_MINOR) SET(OUT "${OUT}.${${PROJECT_NAME}_VERSION_MINOR}") IF (${PROJECT_NAME}_VERSION_PATCH) - SET(OUT "${OUT}.${${PROJECT_NAME}_VERSION_PATCH}") + SET(OUT "${OUT}.${${PROJECT_NAME}_VERSION_PATCH}") ENDIF() ENDIF() ENDIF() @@ -107,7 +107,7 @@ IF (DOXYGEN_FOUND) ELSE() MESSAGE(FATAL_ERROR "You have not Microsoft Help Compiler") ENDIF() - ENDIF () + ENDIF () INSTALL(DIRECTORY "${PROJECT_BINARY_DIR}/html/" DESTINATION "share/doc/lib${PROJECT_NAME}") diff --git a/src/ar_omp.cpp b/src/ar_omp.cpp index eb55fb8..65e1563 100644 --- a/src/ar_omp.cpp +++ b/src/ar_omp.cpp @@ -1,4 +1,4 @@ #ifndef USE_OMP -int omp_get_max_threads(){return 1;} -int omp_get_thread_num(){return 0;} +int omp_get_max_threads() { return 1; } +int omp_get_thread_num() { return 0; } #endif diff --git a/src/ar_omp.h b/src/ar_omp.h index c022674..db170a1 100644 --- a/src/ar_omp.h +++ b/src/ar_omp.h @@ -33,4 +33,3 @@ or implied, of Rafael Muñoz Salinas. int omp_get_max_threads(); int omp_get_thread_num(); #endif - diff --git a/src/aruco.h b/src/aruco.h index 569b95f..845f877 100644 --- a/src/aruco.h +++ b/src/aruco.h @@ -26,26 +26,31 @@ The views and conclusions contained in the software and documentation are those authors and should not be interpreted as representing official policies, either expressed or implied, of Rafael Muñoz Salinas. - - + + \mainpage ArUco: Augmented Reality library from the University of Cordoba -ArUco is a minimal C++ library for detection of Augmented Reality markers based on OpenCv exclusively. +ArUco is a minimal C++ library for detection of Augmented Reality markers based on OpenCv exclusively. It is an educational project to show student how to detect augmented reality markers and it is provided under BSD license. \section INTRODUCTION INTRODUCTION -The library relies on the use of coded markers. Each marker has an unique code indicated by the black and white colors in it. The libary detect borders, and analyzes into the rectangular regions which of them are likely to be markers. Then, a decoding is performed and if the code is valid, it is considered that the rectangle is a marker. +The library relies on the use of coded markers. Each marker has an unique code indicated by the black and white colors in it. The libary detect borders, and +analyzes into the rectangular regions which of them are likely to be markers. Then, a decoding is performed and if the code is valid, it is considered that the +rectangle is a marker. -The codification included into the marker is a slighly modified version of the Hamming Code. It has a total a 25 bits didived in 5 rows of 5 bits each. So, we have 5 words of 5 bits. Each word, contains only 2 bits of real information, the rest is for and error detection/correction (error correction is yet to be done). As a conclusion, a marker contains 10 bits of real information wich allows 1024 different markers. +The codification included into the marker is a slighly modified version of the Hamming Code. It has a total a 25 bits didived in 5 rows of 5 bits each. So, we +have 5 words of 5 bits. Each word, contains only 2 bits of real information, the rest is for and error detection/correction (error correction is yet to be +done). As a conclusion, a marker contains 10 bits of real information wich allows 1024 different markers. \section BOARDS BOARDS -Aruco allows the possibility to employ board. Boards are markers composed by an array of markers arranged in a known order. The advantages of using boards instead of simple markers are: +Aruco allows the possibility to employ board. Boards are markers composed by an array of markers arranged in a known order. The advantages of using boards +instead of simple markers are: - More robusteness. The misdetection of several markers of the board is not a problem as long as a minimum set of them are detected. - More precision. Since there are a larger number of corners, camera pose estimation becomes more precise. @@ -54,25 +59,32 @@ Aruco allows the possibility to employ board. Boards are markers composed by an The library comes with five applications that will help you to learn how to use the library: - aruco_create_marker: which creates marker and saves it in a jpg file you can print. - - aruco_simple : simple test aplication that detects the markers in a image - - aruco_test: this is the main application for detection. It reads images either from the camera of from a video and detect markers. Additionally, if you provide the intrinsics of the camera(obtained by OpenCv calibration) and the size of the marker in meters, the library calculates the marker intrinsics so that you can easily create your AR applications. + - aruco_simple : simple test aplication that detects the markers in a image + - aruco_test: this is the main application for detection. It reads images either from the camera of from a video and detect markers. Additionally, if you +provide the intrinsics of the camera(obtained by OpenCv calibration) and the size of the marker in meters, the library calculates the marker intrinsics so that +you can easily create your AR applications. - aruco_test_gl: shows how to use the library AR applications using OpenGL for rendering - aruco_create_board: application that helps you to create a board - - aruco_simple_board: simple test aplication that detects a board of markers in a image + - aruco_simple_board: simple test aplication that detects a board of markers in a image - aruco_test_board: application that detects boards - aruco_test_board_gl: application that detects boards and uses OpenGL to draw \section LIBRARY LIBRARY DESCRIPTION: -The ArUco library contents are divided in two main directories. The src directory, which contains the library itself. And the utils directory which contains the applications. +The ArUco library contents are divided in two main directories. The src directory, which contains the library itself. And the utils directory which contains the +applications. -The library main classes are: +The library main classes are: - aruco::CameraParameters: represent the information of the camera that captures the images. Here you must set the calibration info. - aruco::Marker: which represent a marker detected in the image - - aruco::MarkerDetector: that is in charge of deteting the markers in a image Detection is done by simple calling the member funcion ArMarkerDetector::detect(). Additionally, the classes contain members to create the required matrices for rendering using OpenGL. See aruco_test_gl for details - - aruco::BoardConfiguration: A board is an array of markers in a known order. BoardConfiguracion is the class that defines a board by indicating the id of its markers. In addition, it has informacion about the distance between the markers so that extrinsica camera computations can be done. - - aruco::Board: This class defines a board detected in a image. The board has the extrinsic camera parameters as public atributes. In addition, it has a method that allows obtain the matrix for getting its position in OpenGL (see aruco_test_board_gl for details). - - aruco::BoardDetector : This is the class in charge of detecting a board in a image. You must pass to it the set of markers detected by ArMarkerDetector and the BoardConfiguracion of the board you want to detect. This class will do the rest for you, even calculating the camera extrinsics. + - aruco::MarkerDetector: that is in charge of deteting the markers in a image Detection is done by simple calling the member funcion +ArMarkerDetector::detect(). Additionally, the classes contain members to create the required matrices for rendering using OpenGL. See aruco_test_gl for details + - aruco::BoardConfiguration: A board is an array of markers in a known order. BoardConfiguracion is the class that defines a board by indicating the id of +its markers. In addition, it has informacion about the distance between the markers so that extrinsica camera computations can be done. + - aruco::Board: This class defines a board detected in a image. The board has the extrinsic camera parameters as public atributes. In addition, it has a +method that allows obtain the matrix for getting its position in OpenGL (see aruco_test_board_gl for details). + - aruco::BoardDetector : This is the class in charge of detecting a board in a image. You must pass to it the set of markers detected by ArMarkerDetector and +the BoardConfiguracion of the board you want to detect. This class will do the rest for you, even calculating the camera extrinsics. \section COMPILING COMPILING THE LIBRARY: @@ -86,49 +98,54 @@ Go to the aruco library and do >make install (optional) \endverbatim -NOTE ON OPENGL: The library supports eaily the integration with OpenGL. In order to compile with support for OpenGL, you just have installed in your system the develop packages for GL and glut (or freeglut). +NOTE ON OPENGL: The library supports eaily the integration with OpenGL. In order to compile with support for OpenGL, you just have installed in your system the +develop packages for GL and glut (or freeglut). \subsection WINDOWS The library has been compiled using MinGW and codeblocks. Below I describe the best way to compile it that I know. If you know better, please let me know. - step 1) codeblocks -# Download codeblocks. I recommend to download the version 10.5 with mingw included (codeblocks-10.05mingw-setup.exe) - -# Install and set the PATH variable so that the codeblock/mingw/bin directory is included. In my case c:/codeblocks/mingw/bin. This will allow cmake to find the compiler. - -# The codeblock program will not find the mingw path by deafult. So, run codeblocks and go to setting->Compuiler debugger and set the correct path to the MinGW dir. + -# Install and set the PATH variable so that the codeblock/mingw/bin directory is included. In my case c:/codeblocks/mingw/bin. This will allow cmake to +find the compiler. + -# The codeblock program will not find the mingw path by deafult. So, run codeblocks and go to setting->Compuiler debugger and set the correct path to the +MinGW dir. - step 2) cmake -# Download and install the last version of cmake. - step 3) OpenCv - -# Download the source code and compile it using cmake and codeblocks. Note: install the library in C:\ if you want it to be easily detected by cmake afterwards + -# Download the source code and compile it using cmake and codeblocks. Note: install the library in C:\ if you want it to be easily detected by cmake +afterwards - step 4) aruco -# Download and decompress. - -# Open cmake gui application and set the path to the main library directory and also set a path where the project is going to be built. + -# Open cmake gui application and set the path to the main library directory and also set a path where the project is going to be built. -# Generate the codeblock project. -# Open the project with codeblock and compile then, install. The programs will be probably generated into the bin directory -OpenGL: by default, the mingw version installed has not the glut library. So, the opengl programs are not compiled. If you want to compile with OpenGL support, you must install glut, or prefereably freeglut. -Thus, - - Download the library (http://www.martinpayne.me.uk/software/development/GLUT/freeglut-MinGW.zip) for mingw. - - Decompress in a directory X. +OpenGL: by default, the mingw version installed has not the glut library. So, the opengl programs are not compiled. If you want to compile with OpenGL support, +you must install glut, or prefereably freeglut. +Thus, + - Download the library (http://www.martinpayne.me.uk/software/development/GLUT/freeglut-MinGW.zip) for mingw. + - Decompress in a directory X. - Then, rerun cmake setting the variable GLU_PATH to that directory (>cmake .. -DGLUT_PATH="C:\X") - Finally, recompile and test. Indeed, you should move the freeglut.dll to the directory with the binaries or to any other place in the PATH. CONCLUSION: Move to Linux, things are simpler :P - -\section Testing -For testing the applications, the library provides videos and the corresponding camera parameters of these videos. Into the directories you will find information on how to run the examples. - +\section Testing + +For testing the applications, the library provides videos and the corresponding camera parameters of these videos. Into the directories you will find +information on how to run the examples. + \section Final Notes - REQUIREMENTS: OpenCv >= 2.1.0. and OpenGL for (aruco_test_gl and aruco_test_board_gl) - CONTACT: Rafael Munoz-Salinas: rmsalinas@uco.es - This libary is free software and come with no guaratee! - + */ #include "markerdetector.h" #include "boarddetector.h" #include "cvdrawingutils.h" - diff --git a/src/arucofidmarkers.cpp b/src/arucofidmarkers.cpp index cdae497..64c84eb 100644 --- a/src/arucofidmarkers.cpp +++ b/src/arucofidmarkers.cpp @@ -42,54 +42,83 @@ namespace aruco { ************************************/ /** */ -Mat FiducidalMarkers::createMarkerImage(int id,int size,bool addWaterMark) throw (cv::Exception) -{ - Mat marker(size,size, CV_8UC1); +Mat FiducidalMarkers::createMarkerImage(int id, int size, bool addWaterMark, bool locked) throw(cv::Exception) { + Mat marker(size, size, CV_8UC1); marker.setTo(Scalar(0)); - if (0<=id && id<1024) { - //for each line, create - int swidth=size/7; - int ids[4]={0x10,0x17,0x09,0x0e}; - for (int y=0;y<5;y++) { - int index=(id>>2*(4-y)) & 0x0003; - int val=ids[index]; - for (int x=0;x<5;x++) { - Mat roi=marker(Rect((x+1)* swidth,(y+1)* swidth,swidth,swidth)); - if ( ( val>>(4-x) ) & 0x0001 ) roi.setTo(Scalar(255)); - else roi.setTo(Scalar(0)); + if (0 <= id && id < 1024) { + // for each line, create + int swidth = size / 7; + int ids[4] = {0x10, 0x17, 0x09, 0x0e}; + for (int y = 0; y < 5; y++) { + int index = (id >> 2 * (4 - y)) & 0x0003; + int val = ids[index]; + for (int x = 0; x < 5; x++) { + Mat roi = marker(Rect((x + 1) * swidth, (y + 1) * swidth, swidth, swidth)); + if ((val >> (4 - x)) & 0x0001) + roi.setTo(Scalar(255)); + else + roi.setTo(Scalar(0)); } } + } else + throw cv::Exception(9004, "id invalid", "createMarker", __FILE__, __LINE__); + + if (addWaterMark) { + char idcad[30]; + sprintf(idcad, "#%d", id); + float ax = float(size) / 100.; + int linew = 1 + (marker.rows / 500); + cv::putText(marker, idcad, cv::Point(0, marker.rows - marker.rows / 40), cv::FONT_HERSHEY_COMPLEX, ax * 0.15f, cv::Scalar::all(30), linew, CV_AA); } - else throw cv::Exception(9004,"id invalid","createMarker",__FILE__,__LINE__); - if (addWaterMark){ - char idcad[30]; - sprintf(idcad,"#%d",id); - float ax=float(size)/100.; - cv::putText(marker,idcad,cv::Point( 0,marker.rows - marker.rows/40),FONT_HERSHEY_TRIPLEX,ax*0.15f,cv::Scalar::all(30)); + + if (locked) { + // add a locking + int sqSize = float(size) * 0.25; + + cv::Mat lock_marker(cv::Size(size + sqSize * 2, size + sqSize * 2), marker.type()); + // cerr<>2*(4-y)) & 0x0003; - int val=ids[index]; - for (int x=0;x<5;x++) { - if ( ( val>>(4-x) ) & 0x0001 ) marker.at(y,x)=1; - else marker.at(y,x)=0; + if (0 <= id && id < 1024) { + // for each line, create + int ids[4] = {0x10, 0x17, 0x09, 0x0e}; + for (int y = 0; y < 5; y++) { + int index = (id >> 2 * (4 - y)) & 0x0003; + int val = ids[index]; + for (int x = 0; x < 5; x++) { + if ((val >> (4 - x)) & 0x0001) + marker.at< uchar >(y, x) = 1; + else + marker.at< uchar >(y, x) = 0; } } - } - else throw cv::Exception (9189,"Invalid marker id","aruco::fiducidal::createMarkerMat",__FILE__,__LINE__); + } else + throw cv::Exception(9189, "Invalid marker id", "aruco::fiducidal::createMarkerMat", __FILE__, __LINE__); return marker; } /************************************ @@ -99,40 +128,41 @@ cv::Mat FiducidalMarkers::getMarkerMat(int id) throw (cv::Exception) * ************************************/ -cv::Mat FiducidalMarkers::createBoardImage( Size gridSize,int MarkerSize,int MarkerDistance, BoardConfiguration& TInfo ,vector *excludedIds) throw (cv::Exception) -{ +cv::Mat FiducidalMarkers::createBoardImage(Size gridSize, int MarkerSize, int MarkerDistance, BoardConfiguration &TInfo, + vector< int > *excludedIds) throw(cv::Exception) { - srand(cv::getTickCount()); - int nMarkers=gridSize.height*gridSize.width; + srand(cv::getTickCount()); + int nMarkers = gridSize.height * gridSize.width; TInfo.resize(nMarkers); - vector ids=getListOfValidMarkersIds_random(nMarkers,excludedIds); - for (int i=0;i ids = getListOfValidMarkersIds_random(nMarkers, excludedIds); + for (int i = 0; i < nMarkers; i++) + TInfo[i].id = ids[i]; + + int sizeY = gridSize.height * MarkerSize + (gridSize.height - 1) * MarkerDistance; + int sizeX = gridSize.width * MarkerSize + (gridSize.width - 1) * MarkerDistance; + // find the center so that the ref systeem is in it + int centerX = sizeX / 2; + int centerY = sizeY / 2; + + // indicate the data is expressed in pixels + TInfo.mInfoType = BoardConfiguration::PIX; + Mat tableImage(sizeY, sizeX, CV_8UC1); tableImage.setTo(Scalar(255)); - int idp=0; - for (int y=0;y *excludedIds) throw (cv::Exception) -{ +cv::Mat FiducidalMarkers::createBoardImage_ChessBoard(Size gridSize, int MarkerSize, BoardConfiguration &TInfo, bool centerData, + vector< int > *excludedIds) throw(cv::Exception) { srand(cv::getTickCount()); - //determine the total number of markers required - int nMarkers= 3*(gridSize.width*gridSize.height)/4;//overdetermine the number of marker read - vector idsVector=getListOfValidMarkersIds_random(nMarkers,excludedIds); + // determine the total number of markers required + int nMarkers = 3 * (gridSize.width * gridSize.height) / 4; // overdetermine the number of marker read + vector< int > idsVector = getListOfValidMarkersIds_random(nMarkers, excludedIds); - int sizeY=gridSize.height*MarkerSize; - int sizeX=gridSize.width*MarkerSize; - //find the center so that the ref systeem is in it - int centerX=sizeX/2; - int centerY=sizeY/2; + int sizeY = gridSize.height * MarkerSize; + int sizeX = gridSize.width * MarkerSize; + // find the center so that the ref systeem is in it + int centerX = sizeX / 2; + int centerY = sizeY / 2; - Mat tableImage(sizeY,sizeX,CV_8UC1); + Mat tableImage(sizeY, sizeX, CV_8UC1); tableImage.setTo(Scalar(255)); - TInfo.mInfoType=BoardConfiguration::PIX; - int CurMarkerIdx=0; - for (int y=0;y=idsVector.size()) throw cv::Exception(999," FiducidalMarkers::createBoardImage_ChessBoard","INTERNAL ERROR. REWRITE THIS!!",__FILE__,__LINE__); - TInfo.push_back( MarkerInfo(idsVector[CurMarkerIdx++])); + if (CurMarkerIdx >= idsVector.size()) + throw cv::Exception(999, " FiducidalMarkers::createBoardImage_ChessBoard", "INTERNAL ERROR. REWRITE THIS!!", __FILE__, __LINE__); + TInfo.push_back(MarkerInfo(idsVector[CurMarkerIdx++])); - Mat subrect(tableImage,Rect( x*MarkerSize,y*MarkerSize,MarkerSize,MarkerSize)); - Mat marker=createMarkerImage( TInfo.back().id,MarkerSize); - //set the location of the corners + Mat subrect(tableImage, Rect(x * MarkerSize, y * MarkerSize, MarkerSize, MarkerSize)); + Mat marker = createMarkerImage(TInfo.back().id, MarkerSize); + // set the location of the corners TInfo.back().resize(4); - TInfo.back()[0]=cv::Point3f( x*(MarkerSize),y*(MarkerSize),0); - TInfo.back()[1]=cv::Point3f( x*(MarkerSize)+MarkerSize,y*(MarkerSize),0); - TInfo.back()[2]=cv::Point3f( x*(MarkerSize)+MarkerSize,y*(MarkerSize)+MarkerSize,0); - TInfo.back()[3]=cv::Point3f( x*(MarkerSize),y*(MarkerSize)+MarkerSize,0); + TInfo.back()[0] = cv::Point3f(x * (MarkerSize), y * (MarkerSize), 0); + TInfo.back()[1] = cv::Point3f(x * (MarkerSize)+MarkerSize, y * (MarkerSize), 0); + TInfo.back()[2] = cv::Point3f(x * (MarkerSize)+MarkerSize, y * (MarkerSize)+MarkerSize, 0); + TInfo.back()[3] = cv::Point3f(x * (MarkerSize), y * (MarkerSize)+MarkerSize, 0); if (centerData) { - for (int i=0;i<4;i++) - TInfo.back()[i]-=cv::Point3f(centerX,centerY,0); + for (int i = 0; i < 4; i++) + TInfo.back()[i] -= cv::Point3f(centerX, centerY, 0); } marker.copyTo(subrect); } @@ -205,44 +238,43 @@ cv::Mat FiducidalMarkers::createBoardImage_ChessBoard( Size gridSize,int Marker * * ************************************/ -cv::Mat FiducidalMarkers::createBoardImage_Frame( Size gridSize,int MarkerSize,int MarkerDistance, BoardConfiguration& TInfo ,bool centerData,vector *excludedIds ) throw (cv::Exception) -{ +cv::Mat FiducidalMarkers::createBoardImage_Frame(Size gridSize, int MarkerSize, int MarkerDistance, BoardConfiguration &TInfo, bool centerData, + vector< int > *excludedIds) throw(cv::Exception) { + - srand(cv::getTickCount()); - int nMarkers=2*gridSize.height*2*gridSize.width; - vector idsVector=getListOfValidMarkersIds_random(nMarkers,excludedIds); + int nMarkers = 2 * gridSize.height * 2 * gridSize.width; + vector< int > idsVector = getListOfValidMarkersIds_random(nMarkers, excludedIds); - int sizeY=gridSize.height*MarkerSize+MarkerDistance*(gridSize.height-1); - int sizeX=gridSize.width*MarkerSize+MarkerDistance*(gridSize.width-1); - //find the center so that the ref systeem is in it - int centerX=sizeX/2; - int centerY=sizeY/2; + int sizeY = gridSize.height * MarkerSize + MarkerDistance * (gridSize.height - 1); + int sizeX = gridSize.width * MarkerSize + MarkerDistance * (gridSize.width - 1); + // find the center so that the ref systeem is in it + int centerX = sizeX / 2; + int centerY = sizeY / 2; - Mat tableImage(sizeY,sizeX,CV_8UC1); + Mat tableImage(sizeY, sizeX, CV_8UC1); tableImage.setTo(Scalar(255)); - TInfo.mInfoType=BoardConfiguration::PIX; - int CurMarkerIdx=0; - int mSize=MarkerSize+MarkerDistance; - for (int y=0;y(i,j)=in.at(in.cols-j-1,i); + for (int i = 0; i < in.rows; i++) { + for (int j = 0; j < in.cols; j++) { + out.at< uchar >(i, j) = in.at< uchar >(in.cols - j - 1, i); } } return out; @@ -276,42 +305,23 @@ Mat FiducidalMarkers::rotate(const Mat &in) * * ************************************/ -int FiducidalMarkers::hammDistMarker(Mat bits) -{ - int ids[4][5]= - { - { - 1,0,0,0,0 - } - , - { - 1,0,1,1,1 - } - , - { - 0,1,0,0,1 +int FiducidalMarkers::hammDistMarker(Mat bits) { + int ids[4][5] = {{1, 0, 0, 0, 0}, {1, 0, 1, 1, 1}, {0, 1, 0, 0, 1}, {0, 1, 1, 1, 0}}; + int dist = 0; + + for (int y = 0; y < 5; y++) { + int minSum = 1e5; + // hamming distance to each possible word + for (int p = 0; p < 4; p++) { + int sum = 0; + // now, count + for (int x = 0; x < 5; x++) + sum += bits.at< uchar >(y, x) == ids[p][x] ? 0 : 1; + if (minSum > sum) + minSum = sum; } - , - { - 0, 1, 1, 1, 0 - } - }; - int dist=0; - - for (int y=0;y<5;y++) - { - int minSum=1e5; - //hamming distance to each possible word - for (int p=0;p<4;p++) - { - int sum=0; - //now, count - for (int x=0;x<5;x++) - sum+= bits.at(y,x) == ids[p][x]?0:1; - if (minSum>sum) minSum=sum; - } - //do the and - dist+=minSum; + // do the and + dist += minSum; } return dist; @@ -323,83 +333,79 @@ int FiducidalMarkers::hammDistMarker(Mat bits) * * ************************************/ -int FiducidalMarkers::analyzeMarkerImage(Mat &grey,int &nRotations) -{ - - //Markers are divided in 7x7 regions, of which the inner 5x5 belongs to marker info - //the external border shoould be entirely black - - int swidth=grey.rows/7; - for (int y=0;y<7;y++) - { - int inc=6; - if (y==0 || y==6) inc=1;//for first and last row, check the whole border - for (int x=0;x<7;x+=inc) - { - int Xstart=(x)*(swidth); - int Ystart=(y)*(swidth); - Mat square=grey(Rect(Xstart,Ystart,swidth,swidth)); - int nZ=countNonZero(square); - if (nZ> (swidth*swidth) /2) { -// cout<<"neb"< (swidth * swidth) / 2) { + // cout<<"neb"< markerInfo(5); - Mat _bits=Mat::zeros(5,5,CV_8UC1); - //get information(for each inner square, determine if it is black or white) - - for (int y=0;y<5;y++) - { - - for (int x=0;x<5;x++) - { - int Xstart=(x+1)*(swidth); - int Ystart=(y+1)*(swidth); - Mat square=grey(Rect(Xstart,Ystart,swidth,swidth)); - int nZ=countNonZero(square); - if (nZ> (swidth*swidth) /2) _bits.at( y,x)=1; + // now, + vector< int > markerInfo(5); + Mat _bits = Mat::zeros(5, 5, CV_8UC1); + // get information(for each inner square, determine if it is black or white) + + for (int y = 0; y < 5; y++) { + + for (int x = 0; x < 5; x++) { + int Xstart = (x + 1) * (swidth); + int Ystart = (y + 1) * (swidth); + Mat square = grey(Rect(Xstart, Ystart, swidth, swidth)); + int nZ = countNonZero(square); + if (nZ > (swidth * swidth) / 2) + _bits.at< uchar >(y, x) = 1; } } -// printMat( _bits,"or mat"); + // printMat( _bits,"or mat"); - //checkl all possible rotations + // checkl all possible rotations Mat _bitsFlip; Mat Rotations[4]; - Rotations[0]=_bits; + Rotations[0] = _bits; int dists[4]; - dists[0]=hammDistMarker( Rotations[0]) ; - pair minDist( dists[0],0); - for (int i=1;i<4;i++) - { - //rotate - Rotations[i]=rotate(Rotations[i-1]); - //get the hamming distance to the nearest possible word - dists[i]=hammDistMarker( Rotations[i]) ; - if (dists[i] minDist(dists[0], 0); + for (int i = 1; i < 4; i++) { + // rotate + Rotations[i] = rotate(Rotations[i - 1]); + // get the hamming distance to the nearest possible word + dists[i] = hammDistMarker(Rotations[i]); + if (dists[i] < minDist.first) { + minDist.first = dists[i]; + minDist.second = i; } } -// printMat( Rotations [ minDist.second]); -// cout<<"MinDist="<( Rotations [ minDist.second]); + // cout<<"MinDist="<(y,1)) MatID|=1; - MatID<<=1; - if ( bits.at(y,3)) MatID|=1; + else { // Get id of the marker + int MatID = 0; + cv::Mat bits = Rotations[minDist.second]; + for (int y = 0; y < 5; y++) { + MatID <<= 1; + if (bits.at< uchar >(y, 1)) + MatID |= 1; + MatID <<= 1; + if (bits.at< uchar >(y, 3)) + MatID |= 1; } return MatID; } @@ -412,43 +418,26 @@ int FiducidalMarkers::analyzeMarkerImage(Mat &grey,int &nRotations) * * ************************************/ -bool FiducidalMarkers::correctHammMarker(Mat &bits) -{ - //detect this lines with errors +bool FiducidalMarkers::correctHammMarker(Mat &bits) { + // detect this lines with errors bool errors[4]; - int ids[4][5]= - { - { - 0,0,0,0,0 - } - , - { - 0,0,1,1,1 - } - , - { - 1,1,0,0,1 + int ids[4][5] = {{0, 0, 0, 0, 0}, {0, 0, 1, 1, 1}, {1, 1, 0, 0, 1}, {1, 1, 1, 1, 0}}; + + for (int y = 0; y < 5; y++) { + int minSum = 1e5; + // hamming distance to each possible word + for (int p = 0; p < 4; p++) { + int sum = 0; + // now, count + for (int x = 0; x < 5; x++) + sum += bits.at< uchar >(y, x) == ids[p][x] ? 0 : 1; + if (minSum > sum) + minSum = sum; } - , - { - 1, 1, 1, 1, 0 - } - }; - - for (int y=0;y<5;y++) - { - int minSum=1e5; - //hamming distance to each possible word - for (int p=0;p<4;p++) - { - int sum=0; - //now, count - for (int x=0;x<5;x++) - sum+= bits.at(y,x) == ids[p][x]?0:1; - if (minSum>sum) minSum=sum; - } - if (minSum!=0) errors[y]=true; - else errors[y]=false; + if (minSum != 0) + errors[y] = true; + else + errors[y] = false; } return true; @@ -460,20 +449,22 @@ bool FiducidalMarkers::correctHammMarker(Mat &bits) * * ************************************/ -int FiducidalMarkers::detect(const Mat &in,int &nRotations) -{ - assert(in.rows==in.cols); +int FiducidalMarkers::detect(const Mat &in, int &nRotations) { + assert(in.rows == in.cols); Mat grey; - if ( in.type()==CV_8UC1) grey=in; - else cv::cvtColor(in,grey,CV_BGR2GRAY); - //threshold image - threshold(grey, grey,125, 255, THRESH_BINARY|THRESH_OTSU); - - //now, analyze the interior in order to get the id - //try first with the big ones - - return analyzeMarkerImage(grey,nRotations);; - //too many false positives + if (in.type() == CV_8UC1) + grey = in; + else + cv::cvtColor(in, grey, CV_BGR2GRAY); + // threshold image + threshold(grey, grey, 125, 255, THRESH_BINARY | THRESH_OTSU); + + // now, analyze the interior in order to get the id + // try first with the big ones + + return analyzeMarkerImage(grey, nRotations); + ; + // too many false positives /* int id=analyzeMarkerImage(grey,nRotations); if (id!=-1) return id; id=analyzeMarkerImage_type2(grey,nRotations); @@ -481,31 +472,30 @@ int FiducidalMarkers::detect(const Mat &in,int &nRotations) return -1;*/ } -vector FiducidalMarkers::getListOfValidMarkersIds_random(int nMarkers,vector *excluded) throw (cv::Exception) -{ - - if (excluded!=NULL) - if (nMarkers+excluded->size()>1024) throw cv::Exception(8888,"FiducidalMarkers::getListOfValidMarkersIds_random","Number of possible markers is exceeded",__FILE__,__LINE__); - - vector listOfMarkers(1024); -//set a list with all ids - for (int i=0;i<1024;i++) listOfMarkers[i]=i; - - if (excluded!=NULL)//set excluded to -1 - for (size_t i=0;isize();i++) - listOfMarkers[excluded->at(i)]=-1; -//random shuffle - random_shuffle(listOfMarkers.begin(),listOfMarkers.end()); -//now, take the first nMarkers elements with value !=-1 - int i=0; - vector retList; - while (retList.size() FiducidalMarkers::getListOfValidMarkersIds_random(int nMarkers, vector< int > *excluded) throw(cv::Exception) { + + if (excluded != NULL) + if (nMarkers + excluded->size() > 1024) + throw cv::Exception(8888, "FiducidalMarkers::getListOfValidMarkersIds_random", "Number of possible markers is exceeded", __FILE__, __LINE__); + + vector< int > listOfMarkers(1024); + // set a list with all ids + for (int i = 0; i < 1024; i++) + listOfMarkers[i] = i; + + if (excluded != NULL) // set excluded to -1 + for (size_t i = 0; i < excluded->size(); i++) + listOfMarkers[excluded->at(i)] = -1; + // random shuffle + random_shuffle(listOfMarkers.begin(), listOfMarkers.end()); + // now, take the first nMarkers elements with value !=-1 + int i = 0; + vector< int > retList; + while (retList.size() < nMarkers) { + if (listOfMarkers[i] != -1) retList.push_back(listOfMarkers[i]); - i++; + i++; } return retList; } - } - diff --git a/src/arucofidmarkers.h b/src/arucofidmarkers.h index 59ddae8..7eeee4e 100644 --- a/src/arucofidmarkers.h +++ b/src/arucofidmarkers.h @@ -35,7 +35,7 @@ or implied, of Rafael Muñoz Salinas. namespace aruco { class ARUCO_EXPORTS FiducidalMarkers { -public: + public: /** * \brief Creates an ar marker with the id specified using a modified version of the hamming code. * There are two type of markers: a) These of 10 bits b) these of 3 bits. The latter are employed for applications @@ -62,59 +62,61 @@ class ARUCO_EXPORTS FiducidalMarkers { * Note that : The first bit, is the inverse of the hamming parity. This avoids the 0 0 0 0 0 to be valid * These marker are detected by the function getFiduciadlMarker_Aruco_Type1 * @param writeIdWaterMark if true, writes a watermark with the marker id + * @param locked if true, creates etra rectangles lcoking the corners of the marker (new in version 1.2.6) */ - static cv::Mat createMarkerImage(int id,int size,bool writeIdWaterMark=true) throw (cv::Exception); + static cv::Mat createMarkerImage(int id, int size, bool writeIdWaterMark = true, bool locked = false) throw(cv::Exception); /** Detection of fiducidal aruco markers (10 bits) * @param in input image with the patch that contains the possible marker * @param nRotations number of 90deg rotations in clockwise direction needed to set the marker in correct position * @return -1 if the image passed is a not a valid marker, and its id in case it really is a marker */ - static int detect(const cv::Mat &in,int &nRotations); + static int detect(const cv::Mat &in, int &nRotations); /**Similar to createMarkerImage. Instead of returning a visible image, returns a 8UC1 matrix of 0s and 1s with the marker info */ - static cv::Mat getMarkerMat(int id) throw (cv::Exception); + static cv::Mat getMarkerMat(int id) throw(cv::Exception); /**Creates a printable image of a board * @param gridSize grid layout (numer of sqaures in x and Y) * @param MarkerSize size of markers sides in pixels * @param MarkerDistance distance between the markers - * @param TInfo output + * @param TInfo output * @param excludedIds set of ids excluded from the board */ - static cv::Mat createBoardImage( cv::Size gridSize,int MarkerSize,int MarkerDistance, BoardConfiguration& TInfo ,vector *excludedIds=NULL ) throw (cv::Exception); + static cv::Mat createBoardImage(cv::Size gridSize, int MarkerSize, int MarkerDistance, BoardConfiguration &TInfo, + vector< int > *excludedIds = NULL) throw(cv::Exception); /**Creates a printable image of a board in chessboard_like manner * @param gridSize grid layout (numer of sqaures in x and Y) * @param MarkerSize size of markers sides in pixels - * @param TInfo output + * @param TInfo output * @param setDataCentered indicates if the center is set at the center of the board. Otherwise it is the left-upper corner - * + * */ - static cv::Mat createBoardImage_ChessBoard( cv::Size gridSize,int MarkerSize, BoardConfiguration& TInfo ,bool setDataCentered=true ,vector *excludedIds=NULL) throw (cv::Exception); + static cv::Mat createBoardImage_ChessBoard(cv::Size gridSize, int MarkerSize, BoardConfiguration &TInfo, bool setDataCentered = true, + vector< int > *excludedIds = NULL) throw(cv::Exception); - /**Creates a printable image of a board in a frame fashion + /**Creates a printable image of a board in a frame fashion * @param gridSize grid layout (numer of sqaures in x and Y) * @param MarkerSize size of markers sides in pixels * @param MarkerDistance distance between the markers - * @param TInfo output + * @param TInfo output * @param setDataCentered indicates if the center is set at the center of the board. Otherwise it is the left-upper corner - * + * */ - static cv::Mat createBoardImage_Frame( cv::Size gridSize,int MarkerSize,int MarkerDistance, BoardConfiguration& TInfo ,bool setDataCentered=true,vector *excludedIds=NULL ) throw (cv::Exception); - -private: - - static vector getListOfValidMarkersIds_random(int nMarkers,vector *excluded) throw (cv::Exception); - static cv::Mat rotate(const cv::Mat & in); - static int hammDistMarker(cv::Mat bits); - static int analyzeMarkerImage(cv::Mat &grey,int &nRotations); - static bool correctHammMarker(cv::Mat &bits); + static cv::Mat createBoardImage_Frame(cv::Size gridSize, int MarkerSize, int MarkerDistance, BoardConfiguration &TInfo, bool setDataCentered = true, + vector< int > *excludedIds = NULL) throw(cv::Exception); + + private: + static vector< int > getListOfValidMarkersIds_random(int nMarkers, vector< int > *excluded) throw(cv::Exception); + static cv::Mat rotate(const cv::Mat &in); + static int hammDistMarker(cv::Mat bits); + static int analyzeMarkerImage(cv::Mat &grey, int &nRotations); + static bool correctHammMarker(cv::Mat &bits); }; - } #endif diff --git a/src/board.cpp b/src/board.cpp index 1a14074..9c09bd3 100644 --- a/src/board.cpp +++ b/src/board.cpp @@ -32,348 +32,350 @@ using namespace std; using namespace cv; namespace aruco { - /** - * - * - */ - BoardConfiguration::BoardConfiguration() { - mInfoType=NONE; - } - /** - * - * - */ - BoardConfiguration::BoardConfiguration ( string filePath ) throw ( cv::Exception ) { - mInfoType=NONE; - readFromFile ( filePath ); - } - /** - * - * - */ - BoardConfiguration::BoardConfiguration ( const BoardConfiguration &T ) : vector ( T ) { -// MarkersInfo=T.MarkersInfo; - mInfoType=T.mInfoType; - } - - /** - * - * - */ - BoardConfiguration & BoardConfiguration ::operator= ( const BoardConfiguration &T ) { -// MarkersInfo=T.MarkersInfo; - vector::operator= ( T ); - mInfoType=T.mInfoType; - return *this; - } - /** - * - * - */ - void BoardConfiguration::saveToFile ( string sfile ) throw ( cv::Exception ) { - - cv::FileStorage fs ( sfile,cv::FileStorage::WRITE ); - saveToFile ( fs ); - - } - /**Saves the board info to a file - */ - void BoardConfiguration::saveToFile ( cv::FileStorage &fs ) throw ( cv::Exception ) { - fs<<"aruco_bc_nmarkers"<< ( int ) size(); - fs<<"aruco_bc_mInfoType"<< ( int ) mInfoType; - fs<<"aruco_bc_markers"<<"["; - for ( size_t i=0; i(T) { + // MarkersInfo=T.MarkersInfo; + mInfoType = T.mInfoType; +} + +/** +* +* +*/ +BoardConfiguration &BoardConfiguration::operator=(const BoardConfiguration &T) { + // MarkersInfo=T.MarkersInfo; + vector< MarkerInfo >::operator=(T); + mInfoType = T.mInfoType; + return *this; +} +/** +* +* +*/ +void BoardConfiguration::saveToFile(string sfile) throw(cv::Exception) { + + cv::FileStorage fs(sfile, cv::FileStorage::WRITE); + saveToFile(fs); +} +/**Saves the board info to a file +*/ +void BoardConfiguration::saveToFile(cv::FileStorage &fs) throw(cv::Exception) { + fs << "aruco_bc_nmarkers" << (int)size(); + fs << "aruco_bc_mInfoType" << (int)mInfoType; + fs << "aruco_bc_markers" + << "["; + for (size_t i = 0; i < size(); i++) { + fs << "{:" + << "id" << at(i).id; + + fs << "corners" + << "[:"; + for (int c = 0; c < at(i).size(); c++) + fs << at(i)[c]; fs << "]"; + fs << "}"; } - - /** - * - * - */ - void BoardConfiguration::readFromFile ( string sfile ) throw ( cv::Exception ) { - try{ - cv::FileStorage fs ( sfile,cv::FileStorage::READ ); - readFromFile ( fs ); - }catch (std::exception &ex){ - throw cv::Exception ( 81818,"BoardConfiguration::readFromFile",ex.what()+string(" file=)")+sfile ,__FILE__,__LINE__ ); - } + fs << "]"; +} + +/** +* +* +*/ +void BoardConfiguration::readFromFile(string sfile) throw(cv::Exception) { + try { + cv::FileStorage fs(sfile, cv::FileStorage::READ); + readFromFile(fs); + } catch (std::exception &ex) { + throw cv::Exception(81818, "BoardConfiguration::readFromFile", ex.what() + string(" file=)") + sfile, __FILE__, __LINE__); } - - - /**Reads board info from a file - */ - void BoardConfiguration::readFromFile ( cv::FileStorage &fs ) throw ( cv::Exception ) { - int aux=0; - //look for the nmarkers - if ( fs["aruco_bc_nmarkers"].name() !="aruco_bc_nmarkers" ) - throw cv::Exception ( 81818,"BoardConfiguration::readFromFile","invalid file type" ,__FILE__,__LINE__ ); - fs["aruco_bc_nmarkers"]>>aux; - resize ( aux ); - fs["aruco_bc_mInfoType"]>>mInfoType; - cv::FileNode markers=fs["aruco_bc_markers"]; - int i=0; - for ( FileNodeIterator it = markers.begin(); it!=markers.end(); ++it,i++ ) { - at ( i ).id= ( *it ) ["id"]; - FileNode FnCorners= ( *it ) ["corners"]; - for ( FileNodeIterator itc = FnCorners.begin(); itc!=FnCorners.end(); ++itc ) { - vector coordinates3d; - ( *itc ) >>coordinates3d; - if ( coordinates3d.size() !=3 ) - throw cv::Exception ( 81818,"BoardConfiguration::readFromFile","invalid file type 3" ,__FILE__,__LINE__ ); - cv::Point3f point ( coordinates3d[0],coordinates3d[1],coordinates3d[2] ); - at ( i ).push_back ( point ); - } +} + + +/**Reads board info from a file +*/ +void BoardConfiguration::readFromFile(cv::FileStorage &fs) throw(cv::Exception) { + int aux = 0; + // look for the nmarkers + if (fs["aruco_bc_nmarkers"].name() != "aruco_bc_nmarkers") + throw cv::Exception(81818, "BoardConfiguration::readFromFile", "invalid file type", __FILE__, __LINE__); + fs["aruco_bc_nmarkers"] >> aux; + resize(aux); + fs["aruco_bc_mInfoType"] >> mInfoType; + cv::FileNode markers = fs["aruco_bc_markers"]; + int i = 0; + for (FileNodeIterator it = markers.begin(); it != markers.end(); ++it, i++) { + at(i).id = (*it)["id"]; + FileNode FnCorners = (*it)["corners"]; + for (FileNodeIterator itc = FnCorners.begin(); itc != FnCorners.end(); ++itc) { + vector< float > coordinates3d; + (*itc) >> coordinates3d; + if (coordinates3d.size() != 3) + throw cv::Exception(81818, "BoardConfiguration::readFromFile", "invalid file type 3", __FILE__, __LINE__); + cv::Point3f point(coordinates3d[0], coordinates3d[1], coordinates3d[2]); + at(i).push_back(point); } } - - /** - */ - int BoardConfiguration::getIndexOfMarkerId ( int id ) const - { - - for ( size_t i=0; i(i, 0) != -999999) + invalid |= false; + if (Rvec.at< float >(i, 0) != -999999) + invalid |= false; } - - /** - */ - const MarkerInfo& BoardConfiguration::getMarkerInfo ( int id ) const throw ( cv::Exception ) { - for ( size_t i=0; i(i, j); + // now, add the translation + para[0][3] = Tvec.at< float >(0, 0); + para[1][3] = Tvec.at< float >(1, 0); + para[2][3] = Tvec.at< float >(2, 0); + double scale = 1; + + modelview_matrix[0 + 0 * 4] = para[0][0]; + // R1C2 + modelview_matrix[0 + 1 * 4] = para[0][1]; + modelview_matrix[0 + 2 * 4] = para[0][2]; + modelview_matrix[0 + 3 * 4] = para[0][3]; + // R2 + modelview_matrix[1 + 0 * 4] = para[1][0]; + modelview_matrix[1 + 1 * 4] = para[1][1]; + modelview_matrix[1 + 2 * 4] = para[1][2]; + modelview_matrix[1 + 3 * 4] = para[1][3]; + // R3 + modelview_matrix[2 + 0 * 4] = -para[2][0]; + modelview_matrix[2 + 1 * 4] = -para[2][1]; + modelview_matrix[2 + 2 * 4] = -para[2][2]; + modelview_matrix[2 + 3 * 4] = -para[2][3]; + modelview_matrix[3 + 0 * 4] = 0.0; + modelview_matrix[3 + 1 * 4] = 0.0; + modelview_matrix[3 + 2 * 4] = 0.0; + modelview_matrix[3 + 3 * 4] = 1.0; + if (scale != 0.0) { + modelview_matrix[12] *= scale; + modelview_matrix[13] *= scale; + modelview_matrix[14] *= scale; } - - - /** - */ - void Board::glGetModelViewMatrix ( double modelview_matrix[16] ) throw ( cv::Exception ) { - //check if paremeters are valid - bool invalid=false; - for ( int i=0; i<3 && !invalid ; i++ ) { - if ( Tvec.at ( i,0 ) !=-999999 ) invalid|=false; - if ( Rvec.at ( i,0 ) !=-999999 ) invalid|=false; - } - if ( invalid ) throw cv::Exception ( 9002,"extrinsic parameters are not set","Marker::getModelViewMatrix",__FILE__,__LINE__ ); - Mat Rot ( 3,3,CV_32FC1 ),Jacob; - Rodrigues ( Rvec, Rot, Jacob ); - - double para[3][4]; - for ( int i=0; i<3; i++ ) - for ( int j=0; j<3; j++ ) para[i][j]=Rot.at ( i,j ); - //now, add the translation - para[0][3]=Tvec.at ( 0,0 ); - para[1][3]=Tvec.at ( 1,0 ); - para[2][3]=Tvec.at ( 2,0 ); - double scale=1; - - modelview_matrix[0 + 0*4] = para[0][0]; - // R1C2 - modelview_matrix[0 + 1*4] = para[0][1]; - modelview_matrix[0 + 2*4] = para[0][2]; - modelview_matrix[0 + 3*4] = para[0][3]; - // R2 - modelview_matrix[1 + 0*4] = para[1][0]; - modelview_matrix[1 + 1*4] = para[1][1]; - modelview_matrix[1 + 2*4] = para[1][2]; - modelview_matrix[1 + 3*4] = para[1][3]; - // R3 - modelview_matrix[2 + 0*4] = -para[2][0]; - modelview_matrix[2 + 1*4] = -para[2][1]; - modelview_matrix[2 + 2*4] = -para[2][2]; - modelview_matrix[2 + 3*4] = -para[2][3]; - modelview_matrix[3 + 0*4] = 0.0; - modelview_matrix[3 + 1*4] = 0.0; - modelview_matrix[3 + 2*4] = 0.0; - modelview_matrix[3 + 3*4] = 1.0; - if ( scale != 0.0 ) { - modelview_matrix[12] *= scale; - modelview_matrix[13] *= scale; - modelview_matrix[14] *= scale; - } - - +} + + +/**** + * + */ +void Board::OgreGetPoseParameters(double position[3], double orientation[4]) throw(cv::Exception) { + // check if paremeters are valid + bool invalid = false; + for (int i = 0; i < 3 && !invalid; i++) { + if (Tvec.at< float >(i, 0) != -999999) + invalid |= false; + if (Rvec.at< float >(i, 0) != -999999) + invalid |= false; } - - - /**** - * - */ - void Board::OgreGetPoseParameters ( double position[3], double orientation[4] ) throw ( cv::Exception ) { - //check if paremeters are valid - bool invalid=false; - for ( int i=0; i<3 && !invalid ; i++ ) { - if ( Tvec.at ( i,0 ) !=-999999 ) invalid|=false; - if ( Rvec.at ( i,0 ) !=-999999 ) invalid|=false; - } - if ( invalid ) throw cv::Exception ( 9003,"extrinsic parameters are not set","Marker::getModelViewMatrix",__FILE__,__LINE__ ); - - // calculate position vector - position[0] = -Tvec.ptr ( 0 ) [0]; - position[1] = -Tvec.ptr ( 0 ) [1]; - position[2] = +Tvec.ptr ( 0 ) [2]; - - // now calculare orientation quaternion - cv::Mat Rot ( 3,3,CV_32FC1 ); - cv::Rodrigues ( Rvec, Rot ); - - // calculate axes for quaternion - double stAxes[3][3]; - // x axis - stAxes[0][0] = -Rot.at ( 0,0 ); - stAxes[0][1] = -Rot.at ( 1,0 ); - stAxes[0][2] = +Rot.at ( 2,0 ); - // y axis - stAxes[1][0] = -Rot.at ( 0,1 ); - stAxes[1][1] = -Rot.at ( 1,1 ); - stAxes[1][2] = +Rot.at ( 2,1 ); - // for z axis, we use cross product - stAxes[2][0] = stAxes[0][1]*stAxes[1][2] - stAxes[0][2]*stAxes[1][1]; - stAxes[2][1] = - stAxes[0][0]*stAxes[1][2] + stAxes[0][2]*stAxes[1][0]; - stAxes[2][2] = stAxes[0][0]*stAxes[1][1] - stAxes[0][1]*stAxes[1][0]; - - // transposed matrix - double axes[3][3]; - axes[0][0] = stAxes[0][0]; - axes[1][0] = stAxes[0][1]; - axes[2][0] = stAxes[0][2]; - - axes[0][1] = stAxes[1][0]; - axes[1][1] = stAxes[1][1]; - axes[2][1] = stAxes[1][2]; - - axes[0][2] = stAxes[2][0]; - axes[1][2] = stAxes[2][1]; - axes[2][2] = stAxes[2][2]; - - // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes - // article "Quaternion Calculus and Fast Animation". - double fTrace = axes[0][0]+axes[1][1]+axes[2][2]; - double fRoot; - - if ( fTrace > 0.0 ) { - // |w| > 1/2, may as well choose w > 1/2 - fRoot = sqrt ( fTrace + 1.0 ); // 2w - orientation[0] = 0.5*fRoot; - fRoot = 0.5/fRoot; // 1/(4w) - orientation[1] = ( axes[2][1]-axes[1][2] ) *fRoot; - orientation[2] = ( axes[0][2]-axes[2][0] ) *fRoot; - orientation[3] = ( axes[1][0]-axes[0][1] ) *fRoot; - } else { - // |w| <= 1/2 - static unsigned int s_iNext[3] = { 1, 2, 0 }; - unsigned int i = 0; - if ( axes[1][1] > axes[0][0] ) - i = 1; - if ( axes[2][2] > axes[i][i] ) - i = 2; - unsigned int j = s_iNext[i]; - unsigned int k = s_iNext[j]; - - fRoot = sqrt ( axes[i][i]-axes[j][j]-axes[k][k] + 1.0 ); - double* apkQuat[3] = { &orientation[1], &orientation[2], &orientation[3] }; - *apkQuat[i] = 0.5*fRoot; - fRoot = 0.5/fRoot; - orientation[0] = ( axes[k][j]-axes[j][k] ) *fRoot; - *apkQuat[j] = ( axes[j][i]+axes[i][j] ) *fRoot; - *apkQuat[k] = ( axes[k][i]+axes[i][k] ) *fRoot; - } - - + if (invalid) + throw cv::Exception(9003, "extrinsic parameters are not set", "Marker::getModelViewMatrix", __FILE__, __LINE__); + + // calculate position vector + position[0] = -Tvec.ptr< float >(0)[0]; + position[1] = -Tvec.ptr< float >(0)[1]; + position[2] = +Tvec.ptr< float >(0)[2]; + + // now calculare orientation quaternion + cv::Mat Rot(3, 3, CV_32FC1); + cv::Rodrigues(Rvec, Rot); + + // calculate axes for quaternion + double stAxes[3][3]; + // x axis + stAxes[0][0] = -Rot.at< float >(0, 0); + stAxes[0][1] = -Rot.at< float >(1, 0); + stAxes[0][2] = +Rot.at< float >(2, 0); + // y axis + stAxes[1][0] = -Rot.at< float >(0, 1); + stAxes[1][1] = -Rot.at< float >(1, 1); + stAxes[1][2] = +Rot.at< float >(2, 1); + // for z axis, we use cross product + stAxes[2][0] = stAxes[0][1] * stAxes[1][2] - stAxes[0][2] * stAxes[1][1]; + stAxes[2][1] = -stAxes[0][0] * stAxes[1][2] + stAxes[0][2] * stAxes[1][0]; + stAxes[2][2] = stAxes[0][0] * stAxes[1][1] - stAxes[0][1] * stAxes[1][0]; + + // transposed matrix + double axes[3][3]; + axes[0][0] = stAxes[0][0]; + axes[1][0] = stAxes[0][1]; + axes[2][0] = stAxes[0][2]; + + axes[0][1] = stAxes[1][0]; + axes[1][1] = stAxes[1][1]; + axes[2][1] = stAxes[1][2]; + + axes[0][2] = stAxes[2][0]; + axes[1][2] = stAxes[2][1]; + axes[2][2] = stAxes[2][2]; + + // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes + // article "Quaternion Calculus and Fast Animation". + double fTrace = axes[0][0] + axes[1][1] + axes[2][2]; + double fRoot; + + if (fTrace > 0.0) { + // |w| > 1/2, may as well choose w > 1/2 + fRoot = sqrt(fTrace + 1.0); // 2w + orientation[0] = 0.5 * fRoot; + fRoot = 0.5 / fRoot; // 1/(4w) + orientation[1] = (axes[2][1] - axes[1][2]) * fRoot; + orientation[2] = (axes[0][2] - axes[2][0]) * fRoot; + orientation[3] = (axes[1][0] - axes[0][1]) * fRoot; + } else { + // |w| <= 1/2 + static unsigned int s_iNext[3] = {1, 2, 0}; + unsigned int i = 0; + if (axes[1][1] > axes[0][0]) + i = 1; + if (axes[2][2] > axes[i][i]) + i = 2; + unsigned int j = s_iNext[i]; + unsigned int k = s_iNext[j]; + + fRoot = sqrt(axes[i][i] - axes[j][j] - axes[k][k] + 1.0); + double *apkQuat[3] = {&orientation[1], &orientation[2], &orientation[3]}; + *apkQuat[i] = 0.5 * fRoot; + fRoot = 0.5 / fRoot; + orientation[0] = (axes[k][j] - axes[j][k]) * fRoot; + *apkQuat[j] = (axes[j][i] + axes[i][j]) * fRoot; + *apkQuat[k] = (axes[k][i] + axes[i][k]) * fRoot; } +} - /** - */ - void Board::draw ( cv::Mat &im,cv::Scalar color, int lineWidth,bool writeId ) { - for ( size_t i=0; i>aux; - resize ( aux ); - fs["aruco_bo_rvec"]>> Rvec; - fs["aruco_bo_tvec"]>> Tvec; - - cv::FileNode markers=fs["aruco_bo_markers"]; - int i=0; - for ( FileNodeIterator it = markers.begin(); it!=markers.end(); ++it,i++ ) { - at ( i ).id= ( *it ) ["id"]; - int ncorners= ( *it ) ["ncorners"]; - at ( i ).resize ( ncorners ); - FileNode FnCorners= ( *it ) ["corners"]; - int c=0; - for ( FileNodeIterator itc = FnCorners.begin(); itc!=FnCorners.end(); ++itc,c++ ) { - vector coordinates2d; - ( *itc ) >>coordinates2d; - if ( coordinates2d.size() !=2 ) - throw cv::Exception ( 81818,"Board::readFromFile","invalid file type 2" ,__FILE__,__LINE__ ); - cv::Point2f point; - point.x=coordinates2d[0]; - point.y=coordinates2d[1]; - at ( i ).push_back ( point ); - } + fs << "]"; + // save configuration file + conf.saveToFile(fs); + + + + // readFromFile(fs); +} +/**Read this from a file + */ +void Board::readFromFile(string filePath) throw(cv::Exception) { + cv::FileStorage fs(filePath, cv::FileStorage::READ); + if (fs["aruco_bo_nmarkers"].name() != "aruco_bo_nmarkers") + throw cv::Exception(81818, "Board::readFromFile", "invalid file type:", __FILE__, __LINE__); + + + + int aux = 0; + // look for the nmarkers + fs["aruco_bo_nmarkers"] >> aux; + resize(aux); + fs["aruco_bo_rvec"] >> Rvec; + fs["aruco_bo_tvec"] >> Tvec; + + cv::FileNode markers = fs["aruco_bo_markers"]; + int i = 0; + for (FileNodeIterator it = markers.begin(); it != markers.end(); ++it, i++) { + at(i).id = (*it)["id"]; + int ncorners = (*it)["ncorners"]; + at(i).resize(ncorners); + FileNode FnCorners = (*it)["corners"]; + int c = 0; + for (FileNodeIterator itc = FnCorners.begin(); itc != FnCorners.end(); ++itc, c++) { + vector< float > coordinates2d; + (*itc) >> coordinates2d; + if (coordinates2d.size() != 2) + throw cv::Exception(81818, "Board::readFromFile", "invalid file type 2", __FILE__, __LINE__); + cv::Point2f point; + point.x = coordinates2d[0]; + point.y = coordinates2d[1]; + at(i).push_back(point); } - - conf.readFromFile ( fs ); - - - } - - /** - */ - void BoardConfiguration::getIdList ( std::vector< int >& ids, bool append ) const - { - if ( !append ) ids.clear(); - for ( size_t i=0; i &ids, bool append) const { + if (!append) + ids.clear(); + for (size_t i = 0; i < size(); i++) + ids.push_back(at(i).id); +} }; diff --git a/src/board.h b/src/board.h index 8ac49c3..6ef3858 100644 --- a/src/board.h +++ b/src/board.h @@ -37,16 +37,16 @@ namespace aruco { /** * 3d representation of a marker */ -struct ARUCO_EXPORTS MarkerInfo:public vector { +struct ARUCO_EXPORTS MarkerInfo : public vector< cv::Point3f > { MarkerInfo() {} - MarkerInfo(int _id) {id=_id; } - MarkerInfo(const MarkerInfo&MI): vector(MI){id=MI.id; } - MarkerInfo & operator=(const MarkerInfo&MI){ - vector ::operator=(MI); - id=MI.id; - return *this; - } - int id;//maker id + MarkerInfo(int _id) { id = _id; } + MarkerInfo(const MarkerInfo &MI) : vector< cv::Point3f >(MI) { id = MI.id; } + MarkerInfo &operator=(const MarkerInfo &MI) { + vector< cv::Point3f >::operator=(MI); + id = MI.id; + return *this; + } + int id; // maker id }; /**\brief This class defines a board with several markers. @@ -66,12 +66,16 @@ struct ARUCO_EXPORTS MarkerInfo:public vector { */ -class ARUCO_EXPORTS BoardConfiguration: public vector -{ +class ARUCO_EXPORTS BoardConfiguration : public vector< MarkerInfo > { friend class Board; -public: - enum MarkerInfoType {NONE=-1,PIX=0,METERS=1};//indicates if the data in MakersInfo is expressed in meters or in pixels so as to do conversion internally - //variable indicates if the data in MakersInfo is expressed in meters or in pixels so as to do conversion internally + + public: + enum MarkerInfoType { + NONE = -1, + PIX = 0, + METERS = 1 + }; // indicates if the data in MakersInfo is expressed in meters or in pixels so as to do conversion internally + // variable indicates if the data in MakersInfo is expressed in meters or in pixels so as to do conversion internally int mInfoType; /** */ @@ -79,72 +83,67 @@ class ARUCO_EXPORTS BoardConfiguration: public vector /**Loads from file * @param filePath to the config file */ - BoardConfiguration(string filePath)throw (cv::Exception); + BoardConfiguration(string filePath) throw(cv::Exception); /** */ - BoardConfiguration(const BoardConfiguration &T); + BoardConfiguration(const BoardConfiguration &T); /** */ - BoardConfiguration & operator=(const BoardConfiguration &T); + BoardConfiguration &operator=(const BoardConfiguration &T); /**Saves the board info to a file */ - void saveToFile(string sfile)throw (cv::Exception); + void saveToFile(string sfile) throw(cv::Exception); /**Reads board info from a file */ - void readFromFile(string sfile)throw (cv::Exception); + void readFromFile(string sfile) throw(cv::Exception); /**Indicates if the corners are expressed in meters */ - bool isExpressedInMeters()const { - return mInfoType==METERS; - } + bool isExpressedInMeters() const { return mInfoType == METERS; } /**Indicates if the corners are expressed in meters */ - bool isExpressedInPixels()const { - return mInfoType==PIX; - } + bool isExpressedInPixels() const { return mInfoType == PIX; } /**Returns the index of the marker with id indicated, if is in the list */ - int getIndexOfMarkerId(int id)const; + int getIndexOfMarkerId(int id) const; /**Returns the Info of the marker with id specified. If not in the set, throws exception */ - const MarkerInfo& getMarkerInfo(int id)const throw (cv::Exception); - /**Set in the list passed the set of the ids + const MarkerInfo &getMarkerInfo(int id) const throw(cv::Exception); + /**Set in the list passed the set of the ids */ - void getIdList(vector &ids,bool append=true)const; -private: + void getIdList(vector< int > &ids, bool append = true) const; + + private: /**Saves the board info to a file */ - void saveToFile(cv::FileStorage &fs)throw (cv::Exception); + void saveToFile(cv::FileStorage &fs) throw(cv::Exception); /**Reads board info from a file */ - void readFromFile(cv::FileStorage &fs)throw (cv::Exception); + void readFromFile(cv::FileStorage &fs) throw(cv::Exception); }; /** */ -class ARUCO_EXPORTS Board:public vector -{ +class ARUCO_EXPORTS Board : public vector< Marker > { -public: + public: BoardConfiguration conf; - //matrices of rotation and translation respect to the camera - cv::Mat Rvec,Tvec; + // matrices of rotation and translation respect to the camera + cv::Mat Rvec, Tvec; /** */ - Board() - { - Rvec.create(3,1,CV_32FC1); - Tvec.create(3,1,CV_32FC1); - for (int i=0;i<3;i++) - Tvec.at(i,0)=Rvec.at(i,0)=-999999; + Board() { + Rvec.create(3, 1, CV_32FC1); + Tvec.create(3, 1, CV_32FC1); + for (int i = 0; i < 3; i++) + Tvec.at< float >(i, 0) = Rvec.at< float >(i, 0) = -999999; } /**Given the extrinsic camera parameters returns the GL_MODELVIEW matrix for opengl. * Setting this matrix, the reference corrdinate system will be set in this board */ - void glGetModelViewMatrix(double modelview_matrix[16])throw(cv::Exception); + void glGetModelViewMatrix(double modelview_matrix[16]) throw(cv::Exception); /** * Returns position vector and orientation quaternion for an Ogre scene node or entity. @@ -156,19 +155,19 @@ class ARUCO_EXPORTS Board:public vector * mySceneNode->setOrientation( ogreOrient ); * ... */ - void OgreGetPoseParameters( double position[3], double orientation[4] )throw(cv::Exception); + void OgreGetPoseParameters(double position[3], double orientation[4]) throw(cv::Exception); /**Save this from a file */ - void saveToFile(string filePath)throw(cv::Exception); + void saveToFile(string filePath) throw(cv::Exception); /**Read this from a file */ - void readFromFile(string filePath)throw(cv::Exception); + void readFromFile(string filePath) throw(cv::Exception); /**Draws the detected markers */ - void draw(cv::Mat &im,cv::Scalar color, int lineWidth=1,bool writeId=true); + void draw(cv::Mat &im, cv::Scalar color, int lineWidth = 1, bool writeId = true); }; } diff --git a/src/boarddetector.cpp b/src/boarddetector.cpp index 9dbf1ca..d3dbe1f 100644 --- a/src/boarddetector.cpp +++ b/src/boarddetector.cpp @@ -36,193 +36,202 @@ or implied, of Rafael Muñoz Salinas. using namespace std; using namespace cv; namespace aruco { - /** - */ - BoardDetector::BoardDetector ( bool setYPerpendicular ) { - _setYPerpendicular=setYPerpendicular; - _areParamsSet=false; - repj_err_thres=-1; +/** +*/ +BoardDetector::BoardDetector(bool setYPerpendicular) { + _setYPerpendicular = setYPerpendicular; + _areParamsSet = false; + repj_err_thres = -1; +} +/** + * Use if you plan to let this class to perform marker detection too + */ +void BoardDetector::setParams(const BoardConfiguration &bc, const CameraParameters &cp, float markerSizeMeters) { + _camParams = cp; + _markerSize = markerSizeMeters; + _bconf = bc; + _areParamsSet = true; +} +/** +* +* +*/ +void BoardDetector::setParams(const BoardConfiguration &bc) { + _bconf = bc; + _areParamsSet = true; +} + +/** +* +* +*/ +float BoardDetector::detect(const cv::Mat &im) throw(cv::Exception) { + _mdetector.detect(im, _vmarkers); + + float res; + + if (_camParams.isValid()) + res = detect(_vmarkers, _bconf, _boardDetected, _camParams.CameraMatrix, _camParams.Distorsion, _markerSize); + else + res = detect(_vmarkers, _bconf, _boardDetected); + return res; +} +/** +* +* +*/ +float BoardDetector::detect(const vector< Marker > &detectedMarkers, const BoardConfiguration &BConf, Board &Bdetected, const CameraParameters &cp, + float markerSizeMeters) throw(cv::Exception) { + return detect(detectedMarkers, BConf, Bdetected, cp.CameraMatrix, cp.Distorsion, markerSizeMeters); +} +/** +* +* +*/ +float BoardDetector::detect(const vector< Marker > &detectedMarkers, const BoardConfiguration &BConf, Board &Bdetected, Mat camMatrix, Mat distCoeff, + float markerSizeMeters) throw(cv::Exception) { + if (BConf.size() == 0) + throw cv::Exception(8881, "BoardDetector::detect", "Invalid BoardConfig that is empty", __FILE__, __LINE__); + if (BConf[0].size() < 2) + throw cv::Exception(8881, "BoardDetector::detect", "Invalid BoardConfig that is empty 2", __FILE__, __LINE__); + // compute the size of the markers in meters, which is used for some routines(mostly drawing) + float ssize; + if (BConf.mInfoType == BoardConfiguration::PIX && markerSizeMeters > 0) + ssize = markerSizeMeters; + else if (BConf.mInfoType == BoardConfiguration::METERS) { + ssize = cv::norm(BConf[0][0] - BConf[0][1]); } - /** - * Use if you plan to let this class to perform marker detection too - */ - void BoardDetector::setParams ( const BoardConfiguration &bc,const CameraParameters &cp, float markerSizeMeters ) { - _camParams=cp; - _markerSize=markerSizeMeters; - _bconf=bc; - _areParamsSet=true; - } - /** - * - * - */ - void BoardDetector::setParams ( const BoardConfiguration &bc ) { - _bconf=bc; - _areParamsSet=true; - } - - /** - * - * - */ - float BoardDetector::detect ( const cv::Mat &im ) throw ( cv::Exception ) { - _mdetector.detect ( im,_vmarkers ); - - float res; - if ( _camParams.isValid() ) - res=detect ( _vmarkers,_bconf,_boardDetected,_camParams.CameraMatrix,_camParams.Distorsion,_markerSize ); - else res=detect ( _vmarkers,_bconf,_boardDetected ); - return res; - } - /** - * - * - */ - float BoardDetector::detect ( const vector &detectedMarkers,const BoardConfiguration &BConf, Board &Bdetected,const CameraParameters &cp, float markerSizeMeters ) throw ( cv::Exception ) { - return detect ( detectedMarkers, BConf,Bdetected,cp.CameraMatrix,cp.Distorsion,markerSizeMeters ); + // cout<<"markerSizeMeters="< &detectedMarkers,const BoardConfiguration &BConf, Board &Bdetected, Mat camMatrix,Mat distCoeff,float markerSizeMeters ) throw ( cv::Exception ) { - if ( BConf.size() ==0 ) throw cv::Exception ( 8881,"BoardDetector::detect","Invalid BoardConfig that is empty",__FILE__,__LINE__ ); - if ( BConf[0].size() <2 ) throw cv::Exception ( 8881,"BoardDetector::detect","Invalid BoardConfig that is empty 2",__FILE__,__LINE__ ); - //compute the size of the markers in meters, which is used for some routines(mostly drawing) - float ssize; - if ( BConf.mInfoType==BoardConfiguration::PIX && markerSizeMeters>0 ) ssize=markerSizeMeters; - else if ( BConf.mInfoType==BoardConfiguration::METERS ) { - ssize=cv::norm ( BConf[0][0]-BConf[0][1] ); + // copy configuration + Bdetected.conf = BConf; + // + + bool hasEnoughInfoForRTvecCalculation = false; + if (Bdetected.size() >= 1) { + if (camMatrix.rows != 0) { + if (markerSizeMeters > 0 && BConf.mInfoType == BoardConfiguration::PIX) + hasEnoughInfoForRTvecCalculation = true; + else if (BConf.mInfoType == BoardConfiguration::METERS) + hasEnoughInfoForRTvecCalculation = true; } + } - // cout<<"markerSizeMeters="< objPoints; + vector< cv::Point2f > imagePoints; + for (size_t i = 0; i < Bdetected.size(); i++) { + int idx = Bdetected.conf.getIndexOfMarkerId(Bdetected[i].id); + assert(idx != -1); + for (int p = 0; p < 4; p++) { + imagePoints.push_back(Bdetected[i][p]); + const aruco::MarkerInfo &Minfo = Bdetected.conf.getMarkerInfo(Bdetected[i].id); + objPoints.push_back(Minfo[p] * marker_meter_per_pix); + // cout<=1 ) { - if ( camMatrix.rows!=0 ) { - if ( markerSizeMeters>0 && BConf.mInfoType==BoardConfiguration::PIX ) hasEnoughInfoForRTvecCalculation=true; - else if ( BConf.mInfoType==BoardConfiguration::METERS ) hasEnoughInfoForRTvecCalculation=true; + if (distCoeff.total() == 0) + distCoeff = cv::Mat::zeros(1, 4, CV_32FC1); + + // for(size_t i=0;i< imagePoints.size();i++){ + // cout< objPoints_filtered; - vector imagePoints_filtered; - for ( size_t i=0; i objPoints_filtered; + vector< cv::Point2f > imagePoints_filtered; + for (size_t i = 0; i < pointsThatPassTest.size(); i++) { + objPoints_filtered.push_back(objPoints[pointsThatPassTest[i]]); + imagePoints_filtered.push_back(imagePoints[pointsThatPassTest[i]]); } - - //now, rotate 90 deg in X so that Y axis points up - if ( _setYPerpendicular ) - rotateXAxis ( Bdetected.Rvec ); -// cout<(0,0)<<" "<(1,0)<<" "<(2,0)<(0,0)<<" "<(1,0)<<" "<(2,0)< ( 1,1 ) =cos ( angleRad ); - RX.at ( 1,2 ) =-sin ( angleRad ); - RX.at ( 2,1 ) =sin ( angleRad ); - RX.at ( 2,2 ) =cos ( angleRad ); - //now multiply - R=R*RX; - //finally, the the rodrigues back - Rodrigues ( R,rotation ); + // now, rotate 90 deg in X so that Y axis points up + if (_setYPerpendicular) + rotateXAxis(Bdetected.Rvec); + // cout<(0,0)<<" "<(1,0)<<" "<(2,0)<(0,0)<<" "<(1,0)<<" "<(2,0)<(1, 1) = cos(angleRad); + RX.at< float >(1, 2) = -sin(angleRad); + RX.at< float >(2, 1) = sin(angleRad); + RX.at< float >(2, 2) = cos(angleRad); + // now multiply + R = R * RX; + // finally, the the rodrigues back + Rodrigues(R, rotation); +} + +/**Static version (all in one) + */ +Board BoardDetector::detect(const cv::Mat &Image, const BoardConfiguration &bc, const CameraParameters &cp, float markerSizeMeters) { + BoardDetector BD; + BD.setParams(bc, cp, markerSizeMeters); + BD.detect(Image); + return BD.getDetectedBoard(); +} }; - diff --git a/src/boarddetector.h b/src/boarddetector.h index a56e7e5..ef9f7e2 100644 --- a/src/boarddetector.h +++ b/src/boarddetector.h @@ -34,17 +34,16 @@ or implied, of Rafael Muñoz Salinas. #include "markerdetector.h" using namespace std; -namespace aruco -{ +namespace aruco { /**\brief This class detects AR boards * Version 1.2 * There are two modes for board detection. * First, the old way. (You first detect markers with MarkerDetector and then call to detect in this class. - * + * * Second: New mode, marker detection is included in the class * \code - + CameraParameters CP; CP.readFromFile(path_cp) BoardConfiguration BC; @@ -54,46 +53,45 @@ namespace aruco //capture image cv::Mat im; capture_image(im); - + float prob=BD.detect(im); - if (prob>0.3) - CvDrawingUtils::draw3DAxis(im,BD.getDetectedBoard(),CP); - + if (prob>0.3) + CvDrawingUtils::draw3DAxis(im,BD.getDetectedBoard(),CP); + \endcode - * + * */ -class ARUCO_EXPORTS BoardDetector -{ -public: - /** See discussion in @see enableRotateXAxis. - * Do not change unless you know what you are doing - */ - BoardDetector(bool setYPerpendicular=false); - - +class ARUCO_EXPORTS BoardDetector { + public: + /** See discussion in @see enableRotateXAxis. + * Do not change unless you know what you are doing + */ + BoardDetector(bool setYPerpendicular = false); + + /** * Use if you plan to let this class to perform marker detection too */ - void setParams(const BoardConfiguration &bc,const CameraParameters &cp, float markerSizeMeters=-1); + void setParams(const BoardConfiguration &bc, const CameraParameters &cp, float markerSizeMeters = -1); void setParams(const BoardConfiguration &bc); /** * Detect markers, and then, look for the board indicated in setParams() * @return value indicating the likelihood of having found the marker */ - float detect(const cv::Mat &im)throw (cv::Exception); + float detect(const cv::Mat &im) throw(cv::Exception); /**Returns a reference to the board detected */ - Board & getDetectedBoard(){return _boardDetected;} + Board &getDetectedBoard() { return _boardDetected; } /**Returns a reference to the internal marker detector */ - MarkerDetector &getMarkerDetector(){return _mdetector;} + MarkerDetector &getMarkerDetector() { return _mdetector; } /**Returns the vector of markers detected */ - vector &getDetectedMarkers(){return _vmarkers;} - - - //ALTERNATIVE DETECTION METHOD, BASED ON MARKERS PREVIOUSLY DETECTED - + vector< Marker > &getDetectedMarkers() { return _vmarkers; } + + + // ALTERNATIVE DETECTION METHOD, BASED ON MARKERS PREVIOUSLY DETECTED + /** Given the markers detected, determines if there is the board passed * @param detectedMarkers result provided by aruco::ArMarkerDetector * @param BConf the board you want to see if is present @@ -105,50 +103,49 @@ class ARUCO_EXPORTS BoardDetector * @param markerSizeMeters size of the marker sides expressed in meters * @return value indicating the likelihood of having found the marker */ - float detect(const vector &detectedMarkers,const BoardConfiguration &BConf, Board &Bdetected, cv::Mat camMatrix=cv::Mat(),cv::Mat distCoeff=cv::Mat(), float markerSizeMeters=-1 )throw (cv::Exception); - float detect(const vector &detectedMarkers,const BoardConfiguration &BConf, Board &Bdetected,const CameraParameters &cp, float markerSizeMeters=-1 )throw (cv::Exception); - - /**Static version (all in one). Detects the board indicated - * @param Image input image - * @param bc the board you want to see if is present - * @param cp camera parameters - * @param markerSizeMeters size of the marker sides expressed in meters (not needed in the board is expressed in meters) - * @return Board detected - */ - static Board detect(const cv::Mat &Image, const BoardConfiguration &bc,const CameraParameters &cp, float markerSizeMeters=-1); + float detect(const vector< Marker > &detectedMarkers, const BoardConfiguration &BConf, Board &Bdetected, cv::Mat camMatrix = cv::Mat(), + cv::Mat distCoeff = cv::Mat(), float markerSizeMeters = -1) throw(cv::Exception); + float detect(const vector< Marker > &detectedMarkers, const BoardConfiguration &BConf, Board &Bdetected, const CameraParameters &cp, + float markerSizeMeters = -1) throw(cv::Exception); + + /**Static version (all in one). Detects the board indicated + * @param Image input image + * @param bc the board you want to see if is present + * @param cp camera parameters + * @param markerSizeMeters size of the marker sides expressed in meters (not needed in the board is expressed in meters) + * @return Board detected + */ + static Board detect(const cv::Mat &Image, const BoardConfiguration &bc, const CameraParameters &cp, float markerSizeMeters = -1); /** * By default, the Y axis is set to point up. However this is not the default - * operation mode of opencv, which produces the Z axis pointing up instead. + * operation mode of opencv, which produces the Z axis pointing up instead. * So, to achieve this change, we have to rotate the X axis. */ - void setYPerpendicular(bool enable){_setYPerpendicular=enable;} - void setYPerperdicular(bool enable){ setYPerpendicular(enable); } // TODO mark as deprecated - bool isYPerpendicular(){ return _setYPerpendicular; } - + void setYPerpendicular(bool enable) { _setYPerpendicular = enable; } + void setYPerperdicular(bool enable) { setYPerpendicular(enable); } // TODO mark as deprecated + bool isYPerpendicular() { return _setYPerpendicular; } + /**Sets the threshold for reprjection test. Pixels that after estimating the camera location * projects 'repj_err_thres' pixels farther from its original location are discarded as outliers. * By default it is set to -1, meaning that not reprojection test is performed */ - void set_repj_err_thres(float Repj_err_thres){repj_err_thres=Repj_err_thres;} - float get_repj_err_thres ( )const {return repj_err_thres;} - - -private: + void set_repj_err_thres(float Repj_err_thres) { repj_err_thres = Repj_err_thres; } + float get_repj_err_thres() const { return repj_err_thres; } + + + private: void rotateXAxis(cv::Mat &rotation); bool _setYPerpendicular; - + //-- Functionality to detect markers inside bool _areParamsSet; BoardConfiguration _bconf; Board _boardDetected; - float _markerSize,repj_err_thres; + float _markerSize, repj_err_thres; CameraParameters _camParams; - MarkerDetector _mdetector;//internal markerdetector - vector _vmarkers;//markers detected in the call to : float detect(const cv::Mat &im); - + MarkerDetector _mdetector; // internal markerdetector + vector< Marker > _vmarkers; // markers detected in the call to : float detect(const cv::Mat &im); }; - }; #endif - diff --git a/src/cameraparameters.cpp b/src/cameraparameters.cpp index 1ee8338..ba7ae5e 100644 --- a/src/cameraparameters.cpp +++ b/src/cameraparameters.cpp @@ -28,156 +28,163 @@ or implied, of Rafael Muñoz Salinas. #include "cameraparameters.h" #include #include -#include +#include +#include using namespace std; -namespace aruco -{ +namespace aruco { CameraParameters::CameraParameters() { - CameraMatrix=cv::Mat(); - Distorsion=cv::Mat(); - CamSize=cv::Size(-1,-1); + CameraMatrix = cv::Mat(); + Distorsion = cv::Mat(); + CamSize = cv::Size(-1, -1); } /**Creates the object from the info passed * @param cameraMatrix 3x3 matrix (fx 0 cx, 0 fy cy, 0 0 1) * @param distorsionCoeff 4x1 matrix (k1,k2,p1,p2) * @param size image size */ -CameraParameters::CameraParameters(cv::Mat cameraMatrix,cv::Mat distorsionCoeff,cv::Size size) throw(cv::Exception) { - setParams(cameraMatrix,distorsionCoeff,size); +CameraParameters::CameraParameters(cv::Mat cameraMatrix, cv::Mat distorsionCoeff, cv::Size size) throw(cv::Exception) { + setParams(cameraMatrix, distorsionCoeff, size); } /** */ CameraParameters::CameraParameters(const CameraParameters &CI) { CI.CameraMatrix.copyTo(CameraMatrix); CI.Distorsion.copyTo(Distorsion); - CamSize=CI.CamSize; + CamSize = CI.CamSize; } /** */ -CameraParameters & CameraParameters::operator=(const CameraParameters &CI) { +CameraParameters &CameraParameters::operator=(const CameraParameters &CI) { CI.CameraMatrix.copyTo(CameraMatrix); CI.Distorsion.copyTo(Distorsion); - CamSize=CI.CamSize; + CamSize = CI.CamSize; return *this; } /** */ -void CameraParameters::setParams(cv::Mat cameraMatrix,cv::Mat distorsionCoeff,cv::Size size) throw(cv::Exception) -{ - if (cameraMatrix.rows!=3 || cameraMatrix.cols!=3) - throw cv::Exception(9000,"invalid input cameraMatrix","CameraParameters::setParams",__FILE__,__LINE__); - cameraMatrix.convertTo(CameraMatrix,CV_32FC1); - if ( distorsionCoeff.total()<4 || distorsionCoeff.total()>=7 ) - throw cv::Exception(9000,"invalid input distorsionCoeff","CameraParameters::setParams",__FILE__,__LINE__); +void CameraParameters::setParams(cv::Mat cameraMatrix, cv::Mat distorsionCoeff, cv::Size size) throw(cv::Exception) { + if (cameraMatrix.rows != 3 || cameraMatrix.cols != 3) + throw cv::Exception(9000, "invalid input cameraMatrix", "CameraParameters::setParams", __FILE__, __LINE__); + cameraMatrix.convertTo(CameraMatrix, CV_32FC1); + if (distorsionCoeff.total() < 4 || distorsionCoeff.total() >= 7) + throw cv::Exception(9000, "invalid input distorsionCoeff", "CameraParameters::setParams", __FILE__, __LINE__); cv::Mat auxD; - distorsionCoeff.convertTo( Distorsion,CV_32FC1); + distorsionCoeff.convertTo(Distorsion, CV_32FC1); -// Distorsion.create(1,4,CV_32FC1); -// for (int i=0;i<4;i++) -// Distorsion.ptr(0)[i]=auxD.ptr(0)[i]; - - CamSize=size; + // Distorsion.create(1,4,CV_32FC1); + // for (int i=0;i<4;i++) + // Distorsion.ptr(0)[i]=auxD.ptr(0)[i]; + CamSize = size; } /** */ -cv::Point3f CameraParameters::getCameraLocation(cv::Mat Rvec,cv::Mat Tvec) -{ - cv::Mat m33(3,3,CV_32FC1); - cv::Rodrigues(Rvec, m33) ; - - cv::Mat m44=cv::Mat::eye(4,4,CV_32FC1); - for (int i=0;i<3;i++) - for (int j=0;j<3;j++) - m44.at(i,j)=m33.at(i,j); - - //now, add translation information - for (int i=0;i<3;i++) - m44.at(i,3)=Tvec.at(0,i); - //invert the matrix +cv::Point3f CameraParameters::getCameraLocation(cv::Mat Rvec, cv::Mat Tvec) { + cv::Mat m33(3, 3, CV_32FC1); + cv::Rodrigues(Rvec, m33); + + cv::Mat m44 = cv::Mat::eye(4, 4, CV_32FC1); + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + m44.at< float >(i, j) = m33.at< float >(i, j); + + // now, add translation information + for (int i = 0; i < 3; i++) + m44.at< float >(i, 3) = Tvec.at< float >(0, i); + // invert the matrix m44.inv(); - return cv::Point3f( m44.at(0,0),m44.at(0,1),m44.at(0,2)); - + return cv::Point3f(m44.at< float >(0, 0), m44.at< float >(0, 1), m44.at< float >(0, 2)); } /**Reads the camera parameters from file */ -void CameraParameters::readFromFile(string path)throw(cv::Exception) -{ +void CameraParameters::readFromFile(string path) throw(cv::Exception) { ifstream file(path.c_str()); - if (!file) throw cv::Exception(9005,"could not open file:"+path,"CameraParameters::readFromFile",__FILE__,__LINE__); -//Create the matrices - Distorsion=cv::Mat::zeros(4,1,CV_32FC1); - CameraMatrix=cv::Mat::eye(3,3,CV_32FC1); + if (!file) + throw cv::Exception(9005, "could not open file:" + path, "CameraParameters::readFromFile", __FILE__, __LINE__); + // Create the matrices + Distorsion = cv::Mat::zeros(4, 1, CV_32FC1); + CameraMatrix = cv::Mat::eye(3, 3, CV_32FC1); char line[1024]; while (!file.eof()) { - file.getline(line,1024); + file.getline(line, 1024); char cmd[20]; float fval; - if ( sscanf(line,"%s = %f",cmd,&fval)==2) { + if (sscanf(line, "%s = %f", cmd, &fval) == 2) { string scmd(cmd); - if (scmd=="fx") CameraMatrix.at(0,0)=fval; - else if (scmd=="cx") CameraMatrix.at(0,2)=fval; - else if (scmd=="fy") CameraMatrix.at(1,1)=fval; - else if (scmd=="cy") CameraMatrix.at(1,2)=fval; - else if (scmd=="k1") Distorsion.at(0,0)=fval; - else if (scmd=="k2") Distorsion.at(1,0)=fval; - else if (scmd=="p1") Distorsion.at(2,0)=fval; - else if (scmd=="p2") Distorsion.at(3,0)=fval; - else if (scmd=="width") CamSize.width=fval; - else if (scmd=="height") CamSize.height=fval; + if (scmd == "fx") + CameraMatrix.at< float >(0, 0) = fval; + else if (scmd == "cx") + CameraMatrix.at< float >(0, 2) = fval; + else if (scmd == "fy") + CameraMatrix.at< float >(1, 1) = fval; + else if (scmd == "cy") + CameraMatrix.at< float >(1, 2) = fval; + else if (scmd == "k1") + Distorsion.at< float >(0, 0) = fval; + else if (scmd == "k2") + Distorsion.at< float >(1, 0) = fval; + else if (scmd == "p1") + Distorsion.at< float >(2, 0) = fval; + else if (scmd == "p2") + Distorsion.at< float >(3, 0) = fval; + else if (scmd == "width") + CamSize.width = fval; + else if (scmd == "height") + CamSize.height = fval; } } } /**Saves this to a file */ -void CameraParameters::saveToFile(string path,bool inXML)throw(cv::Exception) -{ - if (!isValid()) throw cv::Exception(9006,"invalid object","CameraParameters::saveToFile",__FILE__,__LINE__); +void CameraParameters::saveToFile(string path, bool inXML) throw(cv::Exception) { + if (!isValid()) + throw cv::Exception(9006, "invalid object", "CameraParameters::saveToFile", __FILE__, __LINE__); if (!inXML) { ofstream file(path.c_str()); - if (!file) throw cv::Exception(9006,"could not open file:"+path,"CameraParameters::saveToFile",__FILE__,__LINE__); - file<<"# Aruco 1.0 CameraParameters"<(0,0)*=AxFactor; - CameraMatrix.at(0,2)*=AxFactor; - CameraMatrix.at(1,1)*=AyFactor; - CameraMatrix.at(1,2)*=AyFactor; +void CameraParameters::resize(cv::Size size) throw(cv::Exception) { + if (!isValid()) + throw cv::Exception(9007, "invalid object", "CameraParameters::resize", __FILE__, __LINE__); + if (size == CamSize) + return; + // now, read the camera size + // resize the camera parameters to fit this image size + float AxFactor = float(size.width) / float(CamSize.width); + float AyFactor = float(size.height) / float(CamSize.height); + CameraMatrix.at< float >(0, 0) *= AxFactor; + CameraMatrix.at< float >(0, 2) *= AxFactor; + CameraMatrix.at< float >(1, 1) *= AyFactor; + CameraMatrix.at< float >(1, 2) *= AyFactor; } /**** @@ -186,116 +193,105 @@ void CameraParameters::resize(cv::Size size)throw(cv::Exception) * * */ -void CameraParameters::readFromXMLFile(string filePath)throw(cv::Exception) -{ +void CameraParameters::readFromXMLFile(string filePath) throw(cv::Exception) { cv::FileStorage fs(filePath, cv::FileStorage::READ); - int w=-1,h=-1; - cv::Mat MCamera,MDist; + int w = -1, h = -1; + cv::Mat MCamera, MDist; fs["image_width"] >> w; fs["image_height"] >> h; fs["distortion_coefficients"] >> MDist; fs["camera_matrix"] >> MCamera; - if (MCamera.cols==0 || MCamera.rows==0)throw cv::Exception(9007,"File :"+filePath+" does not contains valid camera matrix","CameraParameters::readFromXML",__FILE__,__LINE__); - if (w==-1 || h==0)throw cv::Exception(9007,"File :"+filePath+" does not contains valid camera dimensions","CameraParameters::readFromXML",__FILE__,__LINE__); + if (MCamera.cols == 0 || MCamera.rows == 0) + throw cv::Exception(9007, "File :" + filePath + " does not contains valid camera matrix", "CameraParameters::readFromXML", __FILE__, __LINE__); + if (w == -1 || h == 0) + throw cv::Exception(9007, "File :" + filePath + " does not contains valid camera dimensions", "CameraParameters::readFromXML", __FILE__, __LINE__); - if (MCamera.type()!=CV_32FC1) MCamera.convertTo(CameraMatrix,CV_32FC1); - else CameraMatrix=MCamera; + if (MCamera.type() != CV_32FC1) + MCamera.convertTo(CameraMatrix, CV_32FC1); + else + CameraMatrix = MCamera; - if (MDist.total()<4) throw cv::Exception(9007,"File :"+filePath+" does not contains valid distortion_coefficients","CameraParameters::readFromXML",__FILE__,__LINE__); - //convert to 32 and get the 4 first elements only + if (MDist.total() < 4) + throw cv::Exception(9007, "File :" + filePath + " does not contains valid distortion_coefficients", "CameraParameters::readFromXML", __FILE__, + __LINE__); + // convert to 32 and get the 4 first elements only cv::Mat mdist32; - MDist.convertTo(mdist32,CV_32FC1); -// Distorsion.create(1,4,CV_32FC1); -// for (int i=0;i<4;i++) -// Distorsion.ptr(0)[i]=mdist32.ptr(0)[i]; - - Distorsion.create(1,5,CV_32FC1); - for (int i=0;i<5;i++) - Distorsion.ptr(0)[i]=mdist32.ptr(0)[i]; - CamSize.width=w; - CamSize.height=h; + MDist.convertTo(mdist32, CV_32FC1); + // Distorsion.create(1,4,CV_32FC1); + // for (int i=0;i<4;i++) + // Distorsion.ptr(0)[i]=mdist32.ptr(0)[i]; + + Distorsion.create(1, 5, CV_32FC1); + for (int i = 0; i < 5; i++) + Distorsion.ptr< float >(0)[i] = mdist32.ptr< float >(0)[i]; + CamSize.width = w; + CamSize.height = h; } /**** * */ -void CameraParameters::glGetProjectionMatrix( cv::Size orgImgSize, cv::Size size,double proj_matrix[16],double gnear,double gfar,bool invert )throw(cv::Exception) -{ - - if (cv::countNonZero(Distorsion)!=0) std::cerr<< "CameraParameters::glGetProjectionMatrix :: The camera has distortion coefficients " <<__FILE__<<" "<<__LINE__<(0,0)*Ax; - double _cx=CameraMatrix.at(0,2)*Ax; - double _fy=CameraMatrix.at(1,1)*Ay; - double _cy=CameraMatrix.at(1,2)*Ay; - double cparam[3][4] = - { - { - _fx, 0, _cx, 0 - }, - {0, _fy, _cy, 0}, - {0, 0, 1, 0} - }; - - argConvGLcpara2( cparam, size.width, size.height, gnear, gfar, proj_matrix, invert ); - +void CameraParameters::glGetProjectionMatrix(cv::Size orgImgSize, cv::Size size, double proj_matrix[16], double gnear, double gfar, + bool invert) throw(cv::Exception) { + + if (cv::countNonZero(Distorsion) != 0) + std::cerr << "CameraParameters::glGetProjectionMatrix :: The camera has distortion coefficients " << __FILE__ << " " << __LINE__ << endl; + if (isValid() == false) + throw cv::Exception(9100, "invalid camera parameters", "CameraParameters::glGetProjectionMatrix", __FILE__, __LINE__); + + // Deterime the rsized info + double Ax = double(size.width) / double(orgImgSize.width); + double Ay = double(size.height) / double(orgImgSize.height); + double _fx = CameraMatrix.at< float >(0, 0) * Ax; + double _cx = CameraMatrix.at< float >(0, 2) * Ax; + double _fy = CameraMatrix.at< float >(1, 1) * Ay; + double _cy = CameraMatrix.at< float >(1, 2) * Ay; + double cparam[3][4] = {{_fx, 0, _cx, 0}, {0, _fy, _cy, 0}, {0, 0, 1, 0}}; + + argConvGLcpara2(cparam, size.width, size.height, gnear, gfar, proj_matrix, invert); } /******************* * * *******************/ -double CameraParameters::norm( double a, double b, double c ) -{ - return( sqrt( a*a + b*b + c*c ) ); -} +double CameraParameters::norm(double a, double b, double c) { return (sqrt(a * a + b * b + c * c)); } /******************* * * *******************/ -double CameraParameters::dot( double a1, double a2, double a3, - double b1, double b2, double b3 ) -{ - return( a1 * b1 + a2 * b2 + a3 * b3 ); -} +double CameraParameters::dot(double a1, double a2, double a3, double b1, double b2, double b3) { return (a1 * b1 + a2 * b2 + a3 * b3); } /******************* * * *******************/ -void CameraParameters::argConvGLcpara2( double cparam[3][4], int width, int height, double gnear, double gfar, double m[16], bool invert )throw(cv::Exception) -{ +void CameraParameters::argConvGLcpara2(double cparam[3][4], int width, int height, double gnear, double gfar, double m[16], bool invert) throw(cv::Exception) { - double icpara[3][4]; - double trans[3][4]; - double p[3][3], q[4][4]; - int i, j; + double icpara[3][4]; + double trans[3][4]; + double p[3][3], q[4][4]; + int i, j; cparam[0][2] *= -1.0; cparam[1][2] *= -1.0; cparam[2][2] *= -1.0; - if ( arParamDecompMat(cparam, icpara, trans) < 0 ) - throw cv::Exception(9002,"parameter error","MarkerDetector::argConvGLcpara2",__FILE__,__LINE__); + if (arParamDecompMat(cparam, icpara, trans) < 0) + throw cv::Exception(9002, "parameter error", "MarkerDetector::argConvGLcpara2", __FILE__, __LINE__); - for ( i = 0; i < 3; i++ ) - { - for ( j = 0; j < 3; j++ ) - { + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { p[i][j] = icpara[i][j] / icpara[2][2]; } } q[0][0] = (2.0 * p[0][0] / width); q[0][1] = (2.0 * p[0][1] / width); - q[0][2] = ((2.0 * p[0][2] / width) - 1.0); + q[0][2] = ((2.0 * p[0][2] / width) - 1.0); q[0][3] = 0.0; q[1][0] = 0.0; @@ -305,7 +301,7 @@ void CameraParameters::argConvGLcpara2( double cparam[3][4], int width, int heig q[2][0] = 0.0; q[2][1] = 0.0; - q[2][2] = (gfar + gnear)/(gfar - gnear); + q[2][2] = (gfar + gnear) / (gfar - gnear); q[2][3] = -2.0 * gfar * gnear / (gfar - gnear); q[3][0] = 0.0; @@ -313,104 +309,79 @@ void CameraParameters::argConvGLcpara2( double cparam[3][4], int width, int heig q[3][2] = 1.0; q[3][3] = 0.0; - for ( i = 0; i < 4; i++ ) - { - for ( j = 0; j < 3; j++ ) - { - m[i+j*4] = q[i][0] * trans[0][j] - + q[i][1] * trans[1][j] - + q[i][2] * trans[2][j]; + for (i = 0; i < 4; i++) { + for (j = 0; j < 3; j++) { + m[i + j * 4] = q[i][0] * trans[0][j] + q[i][1] * trans[1][j] + q[i][2] * trans[2][j]; } - m[i+3*4] = q[i][0] * trans[0][3] - + q[i][1] * trans[1][3] - + q[i][2] * trans[2][3] - + q[i][3]; + m[i + 3 * 4] = q[i][0] * trans[0][3] + q[i][1] * trans[1][3] + q[i][2] * trans[2][3] + q[i][3]; } - if (!invert) - { - m[13]=-m[13] ; - m[1]=-m[1]; - m[5]=-m[5]; - m[9]=-m[9]; + if (!invert) { + m[13] = -m[13]; + m[1] = -m[1]; + m[5] = -m[5]; + m[9] = -m[9]; } - } /******************* * * *******************/ -int CameraParameters::arParamDecompMat( double source[3][4], double cpara[3][4], double trans[3][4] )throw(cv::Exception) -{ - int r, c; - double Cpara[3][4]; - double rem1, rem2, rem3; - - if ( source[2][3] >= 0 ) - { - for ( r = 0; r < 3; r++ ) - { - for ( c = 0; c < 4; c++ ) - { +int CameraParameters::arParamDecompMat(double source[3][4], double cpara[3][4], double trans[3][4]) throw(cv::Exception) { + int r, c; + double Cpara[3][4]; + double rem1, rem2, rem3; + + if (source[2][3] >= 0) { + for (r = 0; r < 3; r++) { + for (c = 0; c < 4; c++) { Cpara[r][c] = source[r][c]; } } - } - else - { - for ( r = 0; r < 3; r++ ) - { - for ( c = 0; c < 4; c++ ) - { + } else { + for (r = 0; r < 3; r++) { + for (c = 0; c < 4; c++) { Cpara[r][c] = -(source[r][c]); } } } - for ( r = 0; r < 3; r++ ) - { - for ( c = 0; c < 4; c++ ) - { + for (r = 0; r < 3; r++) { + for (c = 0; c < 4; c++) { cpara[r][c] = 0.0; } } - cpara[2][2] = norm( Cpara[2][0], Cpara[2][1], Cpara[2][2] ); + cpara[2][2] = norm(Cpara[2][0], Cpara[2][1], Cpara[2][2]); trans[2][0] = Cpara[2][0] / cpara[2][2]; trans[2][1] = Cpara[2][1] / cpara[2][2]; trans[2][2] = Cpara[2][2] / cpara[2][2]; trans[2][3] = Cpara[2][3] / cpara[2][2]; - cpara[1][2] = dot( trans[2][0], trans[2][1], trans[2][2], - Cpara[1][0], Cpara[1][1], Cpara[1][2] ); + cpara[1][2] = dot(trans[2][0], trans[2][1], trans[2][2], Cpara[1][0], Cpara[1][1], Cpara[1][2]); rem1 = Cpara[1][0] - cpara[1][2] * trans[2][0]; rem2 = Cpara[1][1] - cpara[1][2] * trans[2][1]; rem3 = Cpara[1][2] - cpara[1][2] * trans[2][2]; - cpara[1][1] = norm( rem1, rem2, rem3 ); + cpara[1][1] = norm(rem1, rem2, rem3); trans[1][0] = rem1 / cpara[1][1]; trans[1][1] = rem2 / cpara[1][1]; trans[1][2] = rem3 / cpara[1][1]; - cpara[0][2] = dot( trans[2][0], trans[2][1], trans[2][2], - Cpara[0][0], Cpara[0][1], Cpara[0][2] ); - cpara[0][1] = dot( trans[1][0], trans[1][1], trans[1][2], - Cpara[0][0], Cpara[0][1], Cpara[0][2] ); - rem1 = Cpara[0][0] - cpara[0][1]*trans[1][0] - cpara[0][2]*trans[2][0]; - rem2 = Cpara[0][1] - cpara[0][1]*trans[1][1] - cpara[0][2]*trans[2][1]; - rem3 = Cpara[0][2] - cpara[0][1]*trans[1][2] - cpara[0][2]*trans[2][2]; - cpara[0][0] = norm( rem1, rem2, rem3 ); + cpara[0][2] = dot(trans[2][0], trans[2][1], trans[2][2], Cpara[0][0], Cpara[0][1], Cpara[0][2]); + cpara[0][1] = dot(trans[1][0], trans[1][1], trans[1][2], Cpara[0][0], Cpara[0][1], Cpara[0][2]); + rem1 = Cpara[0][0] - cpara[0][1] * trans[1][0] - cpara[0][2] * trans[2][0]; + rem2 = Cpara[0][1] - cpara[0][1] * trans[1][1] - cpara[0][2] * trans[2][1]; + rem3 = Cpara[0][2] - cpara[0][1] * trans[1][2] - cpara[0][2] * trans[2][2]; + cpara[0][0] = norm(rem1, rem2, rem3); trans[0][0] = rem1 / cpara[0][0]; trans[0][1] = rem2 / cpara[0][0]; trans[0][2] = rem3 / cpara[0][0]; - trans[1][3] = (Cpara[1][3] - cpara[1][2]*trans[2][3]) / cpara[1][1]; - trans[0][3] = (Cpara[0][3] - cpara[0][1]*trans[1][3] - - cpara[0][2]*trans[2][3]) / cpara[0][0]; + trans[1][3] = (Cpara[1][3] - cpara[1][2] * trans[2][3]) / cpara[1][1]; + trans[0][3] = (Cpara[0][3] - cpara[0][1] * trans[1][3] - cpara[0][2] * trans[2][3]) / cpara[0][0]; - for ( r = 0; r < 3; r++ ) - { - for ( c = 0; c < 3; c++ ) - { + for (r = 0; r < 3; r++) { + for (c = 0; c < 3; c++) { cpara[r][c] /= cpara[2][2]; } } @@ -422,77 +393,76 @@ int CameraParameters::arParamDecompMat( double source[3][4], double cpara[3][4], /****** * */ -void CameraParameters::OgreGetProjectionMatrix(cv::Size orgImgSize, cv::Size size, double proj_matrix[16], double gnear, double gfar, bool invert) throw(cv::Exception) -{ +void CameraParameters::OgreGetProjectionMatrix(cv::Size orgImgSize, cv::Size size, double proj_matrix[16], double gnear, double gfar, + bool invert) throw(cv::Exception) { double temp_matrix[16]; (*this).glGetProjectionMatrix(orgImgSize, size, temp_matrix, gnear, gfar, invert); - proj_matrix[0]=-temp_matrix[0]; - proj_matrix[1]=-temp_matrix[4]; - proj_matrix[2]=-temp_matrix[8]; - proj_matrix[3]=temp_matrix[12]; - - proj_matrix[4]=-temp_matrix[1]; - proj_matrix[5]=-temp_matrix[5]; - proj_matrix[6]=-temp_matrix[9]; - proj_matrix[7]=temp_matrix[13]; - - proj_matrix[8]=-temp_matrix[2]; - proj_matrix[9]=-temp_matrix[6]; - proj_matrix[10]=-temp_matrix[10]; - proj_matrix[11]=temp_matrix[14]; - - proj_matrix[12]=-temp_matrix[3]; - proj_matrix[13]=-temp_matrix[7]; - proj_matrix[14]=-temp_matrix[11]; - proj_matrix[15]=temp_matrix[15]; + proj_matrix[0] = -temp_matrix[0]; + proj_matrix[1] = -temp_matrix[4]; + proj_matrix[2] = -temp_matrix[8]; + proj_matrix[3] = temp_matrix[12]; + + proj_matrix[4] = -temp_matrix[1]; + proj_matrix[5] = -temp_matrix[5]; + proj_matrix[6] = -temp_matrix[9]; + proj_matrix[7] = temp_matrix[13]; + + proj_matrix[8] = -temp_matrix[2]; + proj_matrix[9] = -temp_matrix[6]; + proj_matrix[10] = -temp_matrix[10]; + proj_matrix[11] = temp_matrix[14]; + + proj_matrix[12] = -temp_matrix[3]; + proj_matrix[13] = -temp_matrix[7]; + proj_matrix[14] = -temp_matrix[11]; + proj_matrix[15] = temp_matrix[15]; } /****** * */ - cv::Mat CameraParameters::getRTMatrix ( const cv::Mat &R_,const cv::Mat &T_ ,int forceType ) { - cv::Mat M; - cv::Mat R,T; - R_.copyTo ( R ); - T_.copyTo ( T ); - if ( R.type() ==CV_64F ) { - assert ( T.type() ==CV_64F ); - cv::Mat Matrix=cv::Mat::eye ( 4,4,CV_64FC1 ); - - cv::Mat R33=cv::Mat ( Matrix,cv::Rect ( 0,0,3,3 ) ); - if ( R.total() ==3 ) { - cv::Rodrigues ( R,R33 ); - } else if ( R.total() ==9 ) { - cv::Mat R64; - R.convertTo ( R64,CV_64F ); - R.copyTo ( R33 ); - } - for ( int i=0; i<3; i++ ) - Matrix.at ( i,3 ) =T.ptr ( 0 ) [i]; - M=Matrix; - } else if ( R.depth() ==CV_32F ) { - cv::Mat Matrix=cv::Mat::eye ( 4,4,CV_32FC1 ); - cv::Mat R33=cv::Mat ( Matrix,cv::Rect ( 0,0,3,3 ) ); - if ( R.total() ==3 ) { - cv::Rodrigues ( R,R33 ); - } else if ( R.total() ==9 ) { - cv::Mat R32; - R.convertTo ( R32,CV_32F ); - R.copyTo ( R33 ); - } - - for ( int i=0; i<3; i++ ) - Matrix.at ( i,3 ) =T.ptr ( 0 ) [i]; - M=Matrix; +cv::Mat CameraParameters::getRTMatrix(const cv::Mat &R_, const cv::Mat &T_, int forceType) { + cv::Mat M; + cv::Mat R, T; + R_.copyTo(R); + T_.copyTo(T); + if (R.type() == CV_64F) { + assert(T.type() == CV_64F); + cv::Mat Matrix = cv::Mat::eye(4, 4, CV_64FC1); + + cv::Mat R33 = cv::Mat(Matrix, cv::Rect(0, 0, 3, 3)); + if (R.total() == 3) { + cv::Rodrigues(R, R33); + } else if (R.total() == 9) { + cv::Mat R64; + R.convertTo(R64, CV_64F); + R.copyTo(R33); } - - if ( forceType==-1 ) return M; - else { - cv::Mat MTyped; - M.convertTo ( MTyped,forceType ); - return MTyped; + for (int i = 0; i < 3; i++) + Matrix.at< double >(i, 3) = T.ptr< double >(0)[i]; + M = Matrix; + } else if (R.depth() == CV_32F) { + cv::Mat Matrix = cv::Mat::eye(4, 4, CV_32FC1); + cv::Mat R33 = cv::Mat(Matrix, cv::Rect(0, 0, 3, 3)); + if (R.total() == 3) { + cv::Rodrigues(R, R33); + } else if (R.total() == 9) { + cv::Mat R32; + R.convertTo(R32, CV_32F); + R.copyTo(R33); } - } + for (int i = 0; i < 3; i++) + Matrix.at< float >(i, 3) = T.ptr< float >(0)[i]; + M = Matrix; + } + if (forceType == -1) + return M; + else { + cv::Mat MTyped; + M.convertTo(MTyped, forceType); + return MTyped; + } +} }; diff --git a/src/cameraparameters.h b/src/cameraparameters.h index fd0e11d..1dc9686 100644 --- a/src/cameraparameters.h +++ b/src/cameraparameters.h @@ -26,73 +26,70 @@ authors and should not be interpreted as representing official policies, either or implied, of Rafael Muñoz Salinas. ********************************/ #ifndef _Aruco_CameraParameters_H -#define _Aruco_CameraParameters_H +#define _Aruco_CameraParameters_H #include "exports.h" #include #include using namespace std; -namespace aruco -{ +namespace aruco { /**\brief Parameters of the camera */ -class ARUCO_EXPORTS CameraParameters -{ -public: - +class ARUCO_EXPORTS CameraParameters { + public: // 3x3 matrix (fx 0 cx, 0 fy cy, 0 0 1) - cv::Mat CameraMatrix; - //4x1 matrix (k1,k2,p1,p2) - cv::Mat Distorsion; - //size of the image + cv::Mat CameraMatrix; + // 4x1 matrix (k1,k2,p1,p2) + cv::Mat Distorsion; + // size of the image cv::Size CamSize; /**Empty constructor */ - CameraParameters() ; + CameraParameters(); /**Creates the object from the info passed * @param cameraMatrix 3x3 matrix (fx 0 cx, 0 fy cy, 0 0 1) * @param distorsionCoeff 4x1 matrix (k1,k2,p1,p2) * @param size image size */ - CameraParameters(cv::Mat cameraMatrix,cv::Mat distorsionCoeff,cv::Size size) throw(cv::Exception); + CameraParameters(cv::Mat cameraMatrix, cv::Mat distorsionCoeff, cv::Size size) throw(cv::Exception); /**Sets the parameters * @param cameraMatrix 3x3 matrix (fx 0 cx, 0 fy cy, 0 0 1) * @param distorsionCoeff 4x1 matrix (k1,k2,p1,p2) * @param size image size */ - void setParams(cv::Mat cameraMatrix,cv::Mat distorsionCoeff,cv::Size size) throw(cv::Exception); + void setParams(cv::Mat cameraMatrix, cv::Mat distorsionCoeff, cv::Size size) throw(cv::Exception); /**Copy constructor */ - CameraParameters(const CameraParameters &CI) ; + CameraParameters(const CameraParameters &CI); /**Indicates whether this object is valid */ - bool isValid()const { - return CameraMatrix.rows!=0 && CameraMatrix.cols!=0 && Distorsion.rows!=0 && Distorsion.cols!=0 && CamSize.width!=-1 && CamSize.height!=-1; + bool isValid() const { + return CameraMatrix.rows != 0 && CameraMatrix.cols != 0 && Distorsion.rows != 0 && Distorsion.cols != 0 && CamSize.width != -1 && CamSize.height != -1; } /**Assign operator */ - CameraParameters & operator=(const CameraParameters &CI); + CameraParameters &operator=(const CameraParameters &CI); /**Reads the camera parameters from a file generated using saveToFile. */ - void readFromFile(string path)throw(cv::Exception); + void readFromFile(string path) throw(cv::Exception); /**Saves this to a file */ - void saveToFile(string path,bool inXML=true)throw(cv::Exception); + void saveToFile(string path, bool inXML = true) throw(cv::Exception); /**Reads from a YAML file generated with the opencv2.2 calibration utility */ - void readFromXMLFile(string filePath)throw(cv::Exception); + void readFromXMLFile(string filePath) throw(cv::Exception); /**Adjust the parameters to the size of the image indicated */ - void resize(cv::Size size)throw(cv::Exception); + void resize(cv::Size size) throw(cv::Exception); /**Returns the location of the camera in the reference system given by the rotation and translation vectors passed * NOT TESTED */ - static cv::Point3f getCameraLocation(cv::Mat Rvec,cv::Mat Tvec); + static cv::Point3f getCameraLocation(cv::Mat Rvec, cv::Mat Tvec); /**Given the intrinsic camera parameters returns the GL_PROJECTION matrix for opengl. * PLease NOTE that when using OpenGL, it is assumed no camera distorsion! So, if it is not true, you should have @@ -102,10 +99,11 @@ class ARUCO_EXPORTS CameraParameters * @param size of the image/window where to render (can be different from the real camera image). Please not that it must be related to CamMatrix * @param proj_matrix output projection matrix to give to opengl * @param gnear,gfar: visible rendering range - * @param invert: indicates if the output projection matrix has to yield a horizontally inverted image because image data has not been stored in the order of glDrawPixels: bottom-to-top. + * @param invert: indicates if the output projection matrix has to yield a horizontally inverted image because image data has not been stored in the order of + *glDrawPixels: bottom-to-top. */ - void glGetProjectionMatrix( cv::Size orgImgSize, cv::Size size,double proj_matrix[16],double gnear,double gfar,bool invert=false )throw(cv::Exception); - + void glGetProjectionMatrix(cv::Size orgImgSize, cv::Size size, double proj_matrix[16], double gnear, double gfar, bool invert = false) throw(cv::Exception); + /** * setup camera for an Ogre project. * Use: @@ -116,26 +114,21 @@ class ARUCO_EXPORTS CameraParameters * ... * As in OpenGL, it assumes no camera distorsion */ - void OgreGetProjectionMatrix( cv::Size orgImgSize, cv::Size size,double proj_matrix[16],double gnear,double gfar,bool invert=false )throw(cv::Exception); - - - /**Returns the 4x4 homogeneous transform matrix from the R and T vectors computed - */ - static cv::Mat getRTMatrix(const cv::Mat &R_,const cv::Mat &T_ ,int forceType); + void OgreGetProjectionMatrix(cv::Size orgImgSize, cv::Size size, double proj_matrix[16], double gnear, double gfar, + bool invert = false) throw(cv::Exception); -private: - //GL routines - static void argConvGLcpara2( double cparam[3][4], int width, int height, double gnear, double gfar, double m[16], bool invert )throw(cv::Exception); - static int arParamDecompMat( double source[3][4], double cpara[3][4], double trans[3][4] )throw(cv::Exception); - static double norm( double a, double b, double c ); - static double dot( double a1, double a2, double a3, - double b1, double b2, double b3 ); + /**Returns the 4x4 homogeneous transform matrix from the R and T vectors computed + */ + static cv::Mat getRTMatrix(const cv::Mat &R_, const cv::Mat &T_, int forceType); + private: + // GL routines + static void argConvGLcpara2(double cparam[3][4], int width, int height, double gnear, double gfar, double m[16], bool invert) throw(cv::Exception); + static int arParamDecompMat(double source[3][4], double cpara[3][4], double trans[3][4]) throw(cv::Exception); + static double norm(double a, double b, double c); + static double dot(double a1, double a2, double a3, double b1, double b2, double b3); }; - } #endif - - diff --git a/src/chromaticmask.cpp b/src/chromaticmask.cpp index 5bd2f50..bab1fca 100755 --- a/src/chromaticmask.cpp +++ b/src/chromaticmask.cpp @@ -33,439 +33,441 @@ or implied, of Rafael Muñoz Salinas. /** */ -EMClassifier::EMClassifier(unsigned int nelements) : _classifier(2, cv::EM::COV_MAT_DIAGONAL, cv::TermCriteria(cv::TermCriteria::COUNT , 4, FLT_EPSILON)) -{ - _nelem = nelements; - _threshProb = 0.0001; - for(unsigned int i=0; i<256; i++) _prob[i] = 0.5; +EMClassifier::EMClassifier(unsigned int nelements) { +#ifdef OPENCV_VERSION_3 + _classifier = cv::ml::EM::create(); + _classifier->setTermCriteria(cv::TermCriteria(cv::TermCriteria::COUNT, 4, FLT_EPSILON)); + _classifier->setClustersNumber(2); + _classifier->setCovarianceMatrixType(cv::ml::EM::COV_MAT_DIAGONAL); +#else + _classifier = cv::EM(2, cv::EM::COV_MAT_DIAGONAL, cv::TermCriteria(cv::TermCriteria::COUNT , 4, FLT_EPSILON)); +#endif + _nelem = nelements; + _threshProb = 0.0001; + for (unsigned int i = 0; i < 256; i++) + _prob[i] = 0.5; } /** */ -void EMClassifier::train() -{ - - // fill histogram - - for(unsigned int i=0; i<256;i++) _histogram[i]=0; - - for(unsigned int i=0; i<_samples.size(); i++) { - uchar val = _samples[i]; - _histogram[val]+=3; - if(val>0) _histogram[val-1]+=2; - if(val<255) _histogram[val+1]+=2; - if(val>1) _histogram[val-2]+=1; - if(val<254) _histogram[val+2]+=1; - } - - double sum=0.; - for(unsigned int i=0; i<256;i++) sum += _histogram[i]; - for(unsigned int i=0; i<256;i++) _histogram[i] /= sum; - - - // discretize histogram to speed up training - unsigned int histCount[256]; - int n=0; - for(unsigned int i=0; i<256; i++) - n+= (histCount[i] = (unsigned int)(_nelem*_histogram[i])); - - if(n<10) return; - - cv::Mat samples(n,1,CV_64FC1); - unsigned int idx=0; - for(unsigned int i=0; i<256; i++) { - for(unsigned int j=0; j(0)[idx] = i; - idx++; +void EMClassifier::train() { + + // fill histogram + + for (unsigned int i = 0; i < 256; i++) + _histogram[i] = 0; + + for (unsigned int i = 0; i < _samples.size(); i++) { + uchar val = _samples[i]; + _histogram[val] += 3; + if (val > 0) + _histogram[val - 1] += 2; + if (val < 255) + _histogram[val + 1] += 2; + if (val > 1) + _histogram[val - 2] += 1; + if (val < 254) + _histogram[val + 2] += 1; } - } - _classifier.train(samples); + double sum = 0.; + for (unsigned int i = 0; i < 256; i++) + sum += _histogram[i]; + for (unsigned int i = 0; i < 256; i++) + _histogram[i] /= sum; + + + // discretize histogram to speed up training + unsigned int histCount[256]; + int n = 0; + for (unsigned int i = 0; i < 256; i++) + n += (histCount[i] = (unsigned int)(_nelem * _histogram[i])); + + if (n < 10) + return; + + cv::Mat samples(n, 1, CV_64FC1); + unsigned int idx = 0; + for (unsigned int i = 0; i < 256; i++) { + for (unsigned int j = 0; j < histCount[i]; j++) { + samples.ptr< double >(0)[idx] = i; + idx++; + } + } - cv::Mat sampleAux(1,1,CV_64FC1); - for(unsigned int i=0; i<256; i++) { - sampleAux.ptr(0)[0] = i; - cv::Vec2d r = _classifier.predict(sampleAux); - _prob[i] = exp(r[0]); - if(_prob[i]>_threshProb) _inside[i]=true; - else _inside[i]=false; - } +#ifdef OPENCV_VERSION_3 + _classifier->trainEM(samples); +#else + _classifier.train(samples); +#endif + + cv::Mat sampleAux(1, 1, CV_64FC1); + for (unsigned int i = 0; i < 256; i++) { + sampleAux.ptr< double >(0)[0] = i; + cv::Mat probs; +#ifdef OPENCV_VERSION_3 + cv::Vec2f r = _classifier->predict2(sampleAux, probs); +#else + cv::Vec2d r = _classifier.predict(sampleAux); +#endif + _prob[i] = exp(r[0]); + if (_prob[i] > _threshProb) + _inside[i] = true; + else + _inside[i] = false; + } +} -} +void ChromaticMask::setParams(unsigned int mc, unsigned int nc, double threshProb, aruco::CameraParameters CP, aruco::BoardConfiguration BC, float markersize) { + if (BC.mInfoType!=aruco::BoardConfiguration::METERS && markersize == -1) { + std::cerr << "Invalid markersize in ChromaticMask::setParams" << std::endl; + return; + } + if (BC.size() == 0) { + std::cerr << "Invalid BoardConfiguration size in ChromaticMask::setParams" << std::endl; + return; + } -void ChromaticMask::setParams(unsigned int mc, unsigned int nc, double threshProb, aruco::CameraParameters CP, aruco::BoardConfiguration BC, float markersize) -{ - if(markersize==-1) { - std::cerr << "Invalid markersize in ChromaticMask::setParams" << std::endl; - return; - } - - if(BC.size()==0) { - std::cerr << "Invalid BoardConfiguration size in ChromaticMask::setParams" << std::endl; - return; - } - - //calculate corners from min and max in BC - cv::Point3f min, max; - min = max = BC[0][0]; - for(unsigned int i=0; i= max.x && BC[i][j].y >= max.y) max = BC[i][j]; + if(BC.mInfoType==aruco::BoardConfiguration::METERS) { + markersize = cv::norm(BC[0][0]-BC[0][1]); } - } - double pixSize = fabs(markersize/(BC[0][1].x - BC[0][0].x)); - min.x *= pixSize; - min.y *= pixSize; - max.x *= pixSize; - max.y *= pixSize; - - // calculate border corners coordinates - vector corners; - corners.push_back(min); - corners.push_back( cv::Point3f(min.x, max.y, 0) ); - corners.push_back(max); - corners.push_back( cv::Point3f(max.x, min.y, 0) ); - - setParams(mc, nc, threshProb, CP, BC, corners); + // calculate corners from min and max in BC + cv::Point3f min, max; + min = max = BC[0][0]; + for (unsigned int i = 0; i < BC.size(); i++) { + for (unsigned int j = 0; j < 4; j++) { + if (BC[i][j].x <= min.x && BC[i][j].y <= min.y) + min = BC[i][j]; + if (BC[i][j].x >= max.x && BC[i][j].y >= max.y) + max = BC[i][j]; + } + } + double pixSize = fabs(markersize / (BC[0][1].x - BC[0][0].x)); + min.x *= pixSize; + min.y *= pixSize; + max.x *= pixSize; + max.y *= pixSize; + + // calculate border corners coordinates + vector< cv::Point3f > corners; + corners.push_back(min); + corners.push_back(cv::Point3f(min.x, max.y, 0)); + corners.push_back(max); + corners.push_back(cv::Point3f(max.x, min.y, 0)); + + setParams(mc, nc, threshProb, CP, BC, corners); } /** */ -void ChromaticMask::setParams(unsigned int mc, unsigned int nc, double threshProb, aruco::CameraParameters CP, aruco::BoardConfiguration BC, vector corners) -{ - _classifiers.resize(mc*nc); - for(unsigned int i=0; i<_classifiers.size(); i++) _classifiers[i].setProb(threshProb); - _mc = mc; - _nc = nc; - _threshProb = threshProb; - _cell_neighbours.resize(mc*nc); - int idx_=0; - //SGJ: revisar, no engo claro sepa bien que es i y j y los puedo estar confundiendo!!! - for(unsigned int j=0; j corners) { + _classifiers.resize(mc * nc); + for (unsigned int i = 0; i < _classifiers.size(); i++) + _classifiers[i].setProb(threshProb); + _mc = mc; + _nc = nc; + _threshProb = threshProb; + _cell_neighbours.resize(mc * nc); + int idx_ = 0; + // SGJ: revisar, no engo claro sepa bien que es i y j y los puedo estar confundiendo!!! + for (unsigned int j = 0; j < nc; j++) + for (unsigned int i = 0; i < mc; i++, idx_++) { + _centers.push_back(cv::Point2f(i + 0.5, j + 0.5)); + for (unsigned int nj = max(j - 1, (unsigned int)0); nj < min(mc, j + 1); nj++) { + for (unsigned int ni = max(i - 1, (unsigned int)0); ni < min(mc, i + 1); ni++) { + size_t tidx = nj * mc + ni; + _cell_neighbours[idx_].push_back(tidx); + } + } + } + + // deterimne the idx of the neighbours + _BD.getMarkerDetector().setThresholdParams(35, 7); + _BD.getMarkerDetector().enableErosion(false); + _BD.getMarkerDetector().setCornerRefinementMethod(aruco::MarkerDetector::LINES); + _BD.setYPerpendicular(false); + + _BD.setParams(BC, CP); + + _objCornerPoints = corners; + _CP = CP; + + _pixelsVector.clear(); + for (unsigned int i = 0; i < CP.CamSize.height / 2; i++) + for (unsigned int j = 0; j < CP.CamSize.width / 2; j++) + _pixelsVector.push_back(cv::Point2f(2 * j, 2 * i)); + + resetMask(); + _cellMap = cv::Mat(CP.CamSize.height, CP.CamSize.width, CV_8UC1, cv::Scalar::all(0)); + _canonicalPos = cv::Mat(CP.CamSize.height, CP.CamSize.width, CV_8UC2); + + _cellCenters.resize(_classifiers.size()); + for (unsigned int i = 0; i < mc; i++) { + for (unsigned int j = 0; j < nc; j++) { + _cellCenters[i * mc + j].x = ((2 * i + 1) * _cellSize) / 2.; + _cellCenters[i * mc + j].y = ((2 * j + 1) * _cellSize) / 2.; + } } - } - } -pair AvrgTime(0,0) ; +pair< double, double > AvrgTime(0, 0); /** */ -void ChromaticMask::calculateGridImage(const aruco::Board &board ) -{ - - // project corner points to image - cv::projectPoints(_objCornerPoints, board.Rvec, board.Tvec, _CP.CameraMatrix, _CP.Distorsion, _imgCornerPoints); - - //obtain the perspective transform - cv::Point2f pointsRes[4],pointsIn[4]; - for ( int i=0;i<4;i++ ) pointsIn[i]=_imgCornerPoints[i]; - pointsRes[0]= ( cv::Point2f ( 0,0 ) ); - pointsRes[1]= cv::Point2f ( _cellSize*_mc-1, 0 ); - pointsRes[2]= cv::Point2f ( _cellSize*_mc-1, _cellSize*_nc-1 ); - pointsRes[3]= cv::Point2f ( 0, _cellSize*_nc-1 ); - _perpTrans=cv::getPerspectiveTransform ( pointsIn,pointsRes ); - -double tick = (double)cv::getTickCount(); - - vector transformedPixels; - cv::perspectiveTransform(_pixelsVector, transformedPixels, _perpTrans); - - AvrgTime.first+=((double)cv::getTickCount()-tick)/cv::getTickFrequency(); AvrgTime.second++; - //cout<<"Time cc detection="<<1000*AvrgTime.first/AvrgTime.second<<" milliseconds"<(_pixelsVector[i].y, _pixelsVector[i].x) = cv::Vec2b(transformedPixels[i].x, transformedPixels[i].y); - transformedPixels[i].x /= _cellSize; - transformedPixels[i].y /= _cellSize; - if( !transformedPixels[i].inside(cellRect) ) { - _cellMap.at( _pixelsVector[i].y, _pixelsVector[i].x ) = 0; - _cellMap.at( _pixelsVector[i].y+1, _pixelsVector[i].x ) = 0; - _cellMap.at( _pixelsVector[i].y, _pixelsVector[i].x+1 ) = 0; - _cellMap.at( _pixelsVector[i].y+1, _pixelsVector[i].x+1 ) = 0; - } - else { - uchar cellNum = (unsigned int)transformedPixels[i].y*_nc + (unsigned int)transformedPixels[i].x; - _cellMap.at( _pixelsVector[i].y, _pixelsVector[i].x ) = 1+cellNum; - _cellMap.at( _pixelsVector[i].y+1, _pixelsVector[i].x ) = 1+cellNum; - _cellMap.at( _pixelsVector[i].y, _pixelsVector[i].x+1 ) = 1+cellNum; - _cellMap.at( _pixelsVector[i].y+1, _pixelsVector[i].x+1 ) = 1+cellNum; - } - } - +void ChromaticMask::calculateGridImage(const aruco::Board &board) { + + // project corner points to image + cv::projectPoints(_objCornerPoints, board.Rvec, board.Tvec, _CP.CameraMatrix, _CP.Distorsion, _imgCornerPoints); + + // obtain the perspective transform + cv::Point2f pointsRes[4], pointsIn[4]; + for (int i = 0; i < 4; i++) + pointsIn[i] = _imgCornerPoints[i]; + pointsRes[0] = (cv::Point2f(0, 0)); + pointsRes[1] = cv::Point2f(_cellSize * _mc - 1, 0); + pointsRes[2] = cv::Point2f(_cellSize * _mc - 1, _cellSize * _nc - 1); + pointsRes[3] = cv::Point2f(0, _cellSize * _nc - 1); + _perpTrans = cv::getPerspectiveTransform(pointsIn, pointsRes); + + double tick = (double)cv::getTickCount(); + + vector< cv::Point2f > transformedPixels; + cv::perspectiveTransform(_pixelsVector, transformedPixels, _perpTrans); + + AvrgTime.first += ((double)cv::getTickCount() - tick) / cv::getTickFrequency(); + AvrgTime.second++; + cout << "Time cc detection=" << 1000 * AvrgTime.first / AvrgTime.second << " milliseconds" << endl; + + + + cv::Rect cellRect(0, 0, _mc, _nc); + for (unsigned int i = 0; i < transformedPixels.size(); i++) { + //_canonicalPos.at(_pixelsVector[i].y, _pixelsVector[i].x) = cv::Vec2b(transformedPixels[i].x, transformedPixels[i].y); + transformedPixels[i].x /= _cellSize; + transformedPixels[i].y /= _cellSize; + if (!transformedPixels[i].inside(cellRect)) { + _cellMap.at< uchar >(_pixelsVector[i].y, _pixelsVector[i].x) = 0; + _cellMap.at< uchar >(_pixelsVector[i].y + 1, _pixelsVector[i].x) = 0; + _cellMap.at< uchar >(_pixelsVector[i].y, _pixelsVector[i].x + 1) = 0; + _cellMap.at< uchar >(_pixelsVector[i].y + 1, _pixelsVector[i].x + 1) = 0; + } else { + uchar cellNum = (unsigned int)transformedPixels[i].y * _nc + (unsigned int)transformedPixels[i].x; + _cellMap.at< uchar >(_pixelsVector[i].y, _pixelsVector[i].x) = 1 + cellNum; + _cellMap.at< uchar >(_pixelsVector[i].y + 1, _pixelsVector[i].x) = 1 + cellNum; + _cellMap.at< uchar >(_pixelsVector[i].y, _pixelsVector[i].x + 1) = 1 + cellNum; + _cellMap.at< uchar >(_pixelsVector[i].y + 1, _pixelsVector[i].x + 1) = 1 + cellNum; + } + } } /** */ -void ChromaticMask::train(const cv::Mat& in, const aruco::Board &board) -{ - calculateGridImage(board); - - for(unsigned int i=0; i<_classifiers.size(); i++) _classifiers[i].clearSamples(); - - for(unsigned int i=0; i(i,j); - if(idx!=0) _classifiers[idx-1].addSample( in.at(i,j) ); +void ChromaticMask::train(const cv::Mat &in, const aruco::Board &board) { + calculateGridImage(board); + + for (unsigned int i = 0; i < _classifiers.size(); i++) + _classifiers[i].clearSamples(); + + for (unsigned int i = 0; i < in.rows; i++) { + for (unsigned int j = 0; j < in.cols; j++) { + uchar idx = _cellMap.at< uchar >(i, j); + if (idx != 0) + _classifiers[idx - 1].addSample(in.at< uchar >(i, j)); + } } - } - - for(unsigned int i=0; i<_classifiers.size(); i++) _classifiers[i].train(); - - -// for(uint i=0; i<_mc; i++) { -// for(uint j=0; j<_nc; j++) { -// vector neighbors; -// for(int k=-1; k<=1; k++) { -// if((i+k)<0 || (i+k)>=_mc) continue; -// for(int l=-1; l<=1; l++) { -// if((j+l)<0 || (j+l)>=_nc) continue; -// neighbors.push_back((i+k)*_mc + (j+l)); -// } -// } -// -// -// for(uint l=0; l<256; l++) { -// _classifiers[i*_mc+j].probConj[l] = 0.; -// for(uint k=0; k neighbors; + // for(int k=-1; k<=1; k++) { + // if((i+k)<0 || (i+k)>=_mc) continue; + // for(int l=-1; l<=1; l++) { + // if((j+l)<0 || (j+l)>=_nc) continue; + // neighbors.push_back((i+k)*_mc + (j+l)); + // } + // } + // + // + // for(uint l=0; l<256; l++) { + // _classifiers[i*_mc+j].probConj[l] = 0.; + // for(uint k=0; k(i); - const uchar* _cellMap_ptr = _cellMap.ptr(i); - for(unsigned int j=0; j _threshProb) _mask.at(i,j)=1; - - // not considering neighbours - if(_classifiers[idx-1].classify( in.at(i,j) )) _mask.at(i,j)=1; -// if(_classifiers[idx-1].probConj[ in.at(i,j)]>_threshProb ) _mask.at(i,j)=1; - - } +void ChromaticMask::classify(const cv::Mat &in, const aruco::Board &board) { + calculateGridImage(board); + + resetMask(); + + for (unsigned int i = 0; i < in.rows; i++) { + const uchar *in_ptr = in.ptr< uchar >(i); + const uchar *_cellMap_ptr = _cellMap.ptr< uchar >(i); + for (unsigned int j = 0; j < in.cols; j++) { + uchar idx = _cellMap_ptr[j]; + if (idx != 0) { + + // considering neighbours + // float prob=0.0; + // float totalW = 0.0; + // cv::Point2d pix(i,j); + // for(uint k=0; k<_classifiers.size()-17; k++) { + // float w = 1./(1.+getDistance(pix,k)); + // totalW += w; + // prob += w*_classifiers[k].getProb(in_ptr[j] ); + // } + // prob /= totalW; + // if(prob > _threshProb) _mask.at(i,j)=1; + + // not considering neighbours + if (_classifiers[idx - 1].classify(in.at< uchar >(i, j))) + _mask.at< uchar >(i, j) = 1; + // if(_classifiers[idx-1].probConj[ in.at(i,j)]>_threshProb ) _mask.at(i,j)=1; + } + } } - } // apply closing to mask - cv::Mat maskClose; - cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3,3)); - cv::morphologyEx(_mask, maskClose, CV_MOP_CLOSE, element); - _mask = maskClose; - + cv::Mat maskClose; + cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3)); + cv::morphologyEx(_mask, maskClose, CV_MOP_CLOSE, element); + _mask = maskClose; } -cv::Rect fitRectToSize(cv::Rect r,cv::Size size){ - - r.x=max(r.x,0); - r.y=max(r.y,0); - int endx=r.x+r.width; - int endy=r.y+r.height; - endx=min(endx,size.width); - endy=min(endy,size.height); - r.width=endx-r.x; - r.height=endy-r.y; - return r; - +cv::Rect fitRectToSize(cv::Rect r, cv::Size size) { + + r.x = max(r.x, 0); + r.y = max(r.y, 0); + int endx = r.x + r.width; + int endy = r.y + r.height; + endx = min(endx, size.width); + endy = min(endy, size.height); + r.width = endx - r.x; + r.height = endy - r.y; + return r; } /** */ -void ChromaticMask::classify2(const cv::Mat& in, const aruco::Board &board) -{ +void ChromaticMask::classify2(const cv::Mat &in, const aruco::Board &board) { _mask.create(_CP.CamSize.height, _CP.CamSize.width, CV_8UC1); _maskAux.create(_CP.CamSize.height, _CP.CamSize.width, CV_8UC1); - _maskAux.setTo(cv::Scalar::all(0)); - - cv::projectPoints(_objCornerPoints, board.Rvec, board.Tvec, _CP.CameraMatrix, _CP.Distorsion, _imgCornerPoints); - //obtain the perspective transform - cv::Point2f pointsRes[4],pointsIn[4]; - for ( int i=0;i<4;i++ ) pointsIn[i]=_imgCornerPoints[i]; - - pointsRes[0]= ( cv::Point2f ( 0,0 ) ); - pointsRes[1]= cv::Point2f ( _mc-1, 0 ); - pointsRes[2]= cv::Point2f ( _mc-1, _nc-1 ); - pointsRes[3]= cv::Point2f ( 0, _nc-1 ); - _perpTrans=cv::getPerspectiveTransform ( pointsIn,pointsRes ); - - cv::Mat pT_32; - _perpTrans.convertTo(pT_32,CV_32F);//RMS: CAMBIA A FLOAT - -// cout << _perpTrans << endl; - - cv::Rect r = cv::boundingRect(_imgCornerPoints); - r=fitRectToSize(r,in.size());//fit rectangle to image limits - float *H=pT_32.ptr(0); -// #pragma omp parallel for - int ny=0; - for(unsigned int y=r.y; y(y); - uchar *_mask_ptr=_maskAux.ptr(y); - int startx=r.x+ny%2;//alternate starting point - for(unsigned int x=startx; x_mc-1 || c.y<0 || c.y>_nc-1) continue; - size_t cell_idx=c.y*_mc+ c.x;//SGJ: revisar si esto esta bien!! - float prob=0.0,totalW=0.0,dist,w; - - for(unsigned int k=0; k<_cell_neighbours[cell_idx].size(); k++) { - dist = fabs(point.x-_centers[k].x)+fabs( point.y-_centers[k].y); - w= (2-dist); - w*=w; -// w=1/(1- sqrt( (point.x-_centers[k].x)*(point.x-_centers[k].x) + (point.y-_centers[k].y)*(point.y-_centers[k].y)); - totalW += w; - prob += w*_classifiers[ _cell_neighbours[cell_idx][k] ] .getProb(in_ptr[x] ); - } - prob /= totalW; -// prob/=float(_classifiers.size()-17); - if(prob > _threshProb) { - _mask_ptr[x]=1; - } - } - } -// // apply closing to mask -// _mask=_maskAux; - cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3,3)); - cv::morphologyEx(_maskAux,_mask, CV_MOP_CLOSE, element); + _maskAux.setTo(cv::Scalar::all(0)); + + cv::projectPoints(_objCornerPoints, board.Rvec, board.Tvec, _CP.CameraMatrix, _CP.Distorsion, _imgCornerPoints); + // obtain the perspective transform + cv::Point2f pointsRes[4], pointsIn[4]; + for (int i = 0; i < 4; i++) + pointsIn[i] = _imgCornerPoints[i]; + + pointsRes[0] = (cv::Point2f(0, 0)); + pointsRes[1] = cv::Point2f(_mc - 1, 0); + pointsRes[2] = cv::Point2f(_mc - 1, _nc - 1); + pointsRes[3] = cv::Point2f(0, _nc - 1); + _perpTrans = cv::getPerspectiveTransform(pointsIn, pointsRes); + + cv::Mat pT_32; + _perpTrans.convertTo(pT_32, CV_32F); // RMS: CAMBIA A FLOAT + + // cout << _perpTrans << endl; + + cv::Rect r = cv::boundingRect(_imgCornerPoints); + r = fitRectToSize(r, in.size()); // fit rectangle to image limits + float *H = pT_32.ptr< float >(0); + // #pragma omp parallel for + int ny = 0; + for (unsigned int y = r.y; y < r.y + r.height; y += 2, ny++) { + const uchar *in_ptr = in.ptr< uchar >(y); + uchar *_mask_ptr = _maskAux.ptr< uchar >(y); + int startx = r.x + ny % 2; // alternate starting point + for (unsigned int x = startx; x < r.x + r.width; x += 2) { + cv::Point2f point; + float _inv_pointz = 1. / (x * H[6] + y * H[7] + H[8]); + point.x = _inv_pointz * (x * H[0] + y * H[1] + H[2]); + point.y = _inv_pointz * (x * H[3] + y * H[4] + H[5]); + cv::Point2i c; + c.x = int(point.x + 0.5); + c.y = int(point.y + 0.5); + if (c.x < 0 || c.x > _mc - 1 || c.y < 0 || c.y > _nc - 1) + continue; + size_t cell_idx = c.y * _mc + c.x; // SGJ: revisar si esto esta bien!! + float prob = 0.0, totalW = 0.0, dist, w; + + for (unsigned int k = 0; k < _cell_neighbours[cell_idx].size(); k++) { + dist = fabs(point.x - _centers[k].x) + fabs(point.y - _centers[k].y); + w = (2 - dist); + w *= w; + // w=1/(1- sqrt( (point.x-_centers[k].x)*(point.x-_centers[k].x) + (point.y-_centers[k].y)*(point.y-_centers[k].y)); + totalW += w; + prob += w * _classifiers[_cell_neighbours[cell_idx][k]].getProb(in_ptr[x]); + } + prob /= totalW; + // prob/=float(_classifiers.size()-17); + if (prob > _threshProb) { + _mask_ptr[x] = 1; + } + } + } + // // apply closing to mask + // _mask=_maskAux; + cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3)); + cv::morphologyEx(_maskAux, _mask, CV_MOP_CLOSE, element); } -void ChromaticMask::update(const cv::Mat& in) -{ - cv::Mat maskCells; - maskCells = _cellMap.mul(_mask); - - for(unsigned int i=0; i<_classifiers.size(); i++) _classifiers[i].clearSamples(); +void ChromaticMask::update(const cv::Mat &in) { + cv::Mat maskCells; + maskCells = _cellMap.mul(_mask); - for(unsigned int i=0; i(i); - const uchar* in_ptr = in.ptr(i); - for(unsigned int j=0; j 50) { - _classifiers[i].train(); + for (unsigned int i = 0; i < maskCells.rows; i++) { + uchar *maskCells_ptr = maskCells.ptr< uchar >(i); + const uchar *in_ptr = in.ptr< uchar >(i); + for (unsigned int j = 0; j < maskCells.cols; j++) { + if (maskCells_ptr[j] != 0) + _classifiers[maskCells_ptr[j] - 1].addSample(in_ptr[j]); + } } - - + for (unsigned int i = 0; i < _classifiers.size(); i++) + if (_classifiers[i].numsamples() > 50) { + _classifiers[i].train(); + } } -void ChromaticMask::resetMask() -{ +void ChromaticMask::resetMask() { - _mask.create(_CP.CamSize.height, _CP.CamSize.width, CV_8UC1); - _mask.setTo(cv::Scalar::all(0)); + _mask.create(_CP.CamSize.height, _CP.CamSize.width, CV_8UC1); + _mask.setTo(cv::Scalar::all(0)); } - - - - - - - - - - - - - - - - - - - - - diff --git a/src/chromaticmask.h b/src/chromaticmask.h index 2cf0dc3..9644c5e 100755 --- a/src/chromaticmask.h +++ b/src/chromaticmask.h @@ -34,6 +34,7 @@ or implied, of Rafael Muñoz Salinas. #include #include #include +#include #include "exports.h" #include "board.h" #include "boarddetector.h" @@ -41,79 +42,77 @@ or implied, of Rafael Muñoz Salinas. class ARUCO_EXPORTS EMClassifier { -public: - EMClassifier(unsigned int nelements=200); - void addSample(uchar s) { _samples.push_back(s); }; - void clearSamples() { _samples.clear(); } ; - void train(); - bool classify(uchar s) { return _inside[s]; }; - double getProb(uchar s) { return _prob[s]; }; - unsigned int numsamples() {return _samples.size();}; - void setProb(double p) { _threshProb = p; } - -// double probConj[256]; - -private: - cv::EM _classifier; - vector _samples; - bool _inside[256]; - double _prob[256]; - double _histogram[256]; - unsigned int _nelem; - double _threshProb; - + public: + EMClassifier(unsigned int nelements = 200); + void addSample(uchar s) { _samples.push_back(s); }; + void clearSamples() { _samples.clear(); }; + void train(); + bool classify(uchar s) { return _inside[s]; }; + double getProb(uchar s) { return _prob[s]; }; + unsigned int numsamples() { return _samples.size(); }; + void setProb(double p) { _threshProb = p; } + + // double probConj[256]; + + private: +#ifdef OPENCV_VERSION_3 + cv::Ptr< cv::ml::EM > _classifier; +#else + cv::EM _classifier; +#endif + vector< uchar > _samples; + bool _inside[256]; + double _prob[256]; + double _histogram[256]; + unsigned int _nelem; + double _threshProb; }; -class ARUCO_EXPORTS ChromaticMask -{ -public: - - ChromaticMask() : _cellSize(20) { _isValid=false; }; - - void setParams(unsigned int mc, unsigned int nc, double threshProb, aruco::CameraParameters CP, aruco::BoardConfiguration BC, vector corners); - void setParams(unsigned int mc, unsigned int nc, double threshProb, aruco::CameraParameters CP, aruco::BoardConfiguration BC, float markersize=-1.); - - void calculateGridImage(const aruco::Board &board); - - cv::Mat getCellMap() { return _cellMap; }; - cv::Mat getMask() { return _mask; }; - - void train(const cv::Mat& in, const aruco::Board &board); - void classify(const cv::Mat& in, const aruco::Board &board); - void classify2(const cv::Mat& in, const aruco::Board &board); - void update(const cv::Mat& in); - - bool isValid() {return _isValid;}; - void resetMask(); - -private: - - double getDistance(cv::Point2d pixel, unsigned int classifier) { - cv::Vec2b canPos = _canonicalPos.at(pixel.y, pixel.x)[0]; - return norm(_cellCenters[classifier] - cv::Point2f(canPos[0], canPos[1]) ); - } - - vector _imgCornerPoints; - vector _objCornerPoints; - cv::Mat _perpTrans; - vector _classifiers; - vector _centers; - vector _pixelsVector; - vector _cellCenters; - vector > _cell_neighbours; - const float _cellSize; - - - unsigned int _mc, _nc; - aruco::BoardDetector _BD; - aruco::CameraParameters _CP; - cv::Mat _canonicalPos, _cellMap, _mask,_maskAux; - bool _isValid; - double _threshProb; - - - +class ARUCO_EXPORTS ChromaticMask { + public: + ChromaticMask() : _cellSize(20) { _isValid = false; }; + + void setParams(unsigned int mc, unsigned int nc, double threshProb, aruco::CameraParameters CP, aruco::BoardConfiguration BC, + vector< cv::Point3f > corners); + void setParams(unsigned int mc, unsigned int nc, double threshProb, aruco::CameraParameters CP, aruco::BoardConfiguration BC, float markersize = -1.); + + void calculateGridImage(const aruco::Board &board); + + cv::Mat getCellMap() { return _cellMap; }; + cv::Mat getMask() { return _mask; }; + + void train(const cv::Mat &in, const aruco::Board &board); + void classify(const cv::Mat &in, const aruco::Board &board); + void classify2(const cv::Mat &in, const aruco::Board &board); + void update(const cv::Mat &in); + + bool isValid() { return _isValid; }; + void resetMask(); + + private: + double getDistance(cv::Point2d pixel, unsigned int classifier) { + cv::Vec2b canPos = _canonicalPos.at< cv::Vec2b >(pixel.y, pixel.x)[0]; + return norm(_cellCenters[classifier] - cv::Point2f(canPos[0], canPos[1])); + } + + vector< cv::Point2f > _imgCornerPoints; + vector< cv::Point3f > _objCornerPoints; + cv::Mat _perpTrans; + vector< EMClassifier > _classifiers; + vector< cv::Point2f > _centers; + vector< cv::Point2f > _pixelsVector; + vector< cv::Point2f > _cellCenters; + vector< vector< size_t > > _cell_neighbours; + const float _cellSize; + + + unsigned int _mc, _nc; + aruco::BoardDetector _BD; + aruco::CameraParameters _CP; + cv::Mat _canonicalPos, _cellMap, _mask, _maskAux; + bool _isValid; + double _threshProb; }; #endif // CHROMATICMASK_H diff --git a/src/cvdrawingutils.cpp b/src/cvdrawingutils.cpp index ed66b93..59d2147 100644 --- a/src/cvdrawingutils.cpp +++ b/src/cvdrawingutils.cpp @@ -28,6 +28,8 @@ or implied, of Rafael Muñoz Salinas. #include "cvdrawingutils.h" #include #include +#include + using namespace cv; namespace aruco { /**** @@ -35,33 +37,32 @@ namespace aruco { * * ****/ -void CvDrawingUtils::draw3dAxis(cv::Mat &Image,Marker &m,const CameraParameters &CP) -{ - - float size=m.ssize*3; - Mat objectPoints (4,3,CV_32FC1); - objectPoints.at(0,0)=0; - objectPoints.at(0,1)=0; - objectPoints.at(0,2)=0; - objectPoints.at(1,0)=size; - objectPoints.at(1,1)=0; - objectPoints.at(1,2)=0; - objectPoints.at(2,0)=0; - objectPoints.at(2,1)=size; - objectPoints.at(2,2)=0; - objectPoints.at(3,0)=0; - objectPoints.at(3,1)=0; - objectPoints.at(3,2)=size; - - vector imagePoints; - cv::projectPoints( objectPoints, m.Rvec,m.Tvec, CP.CameraMatrix,CP.Distorsion, imagePoints); -//draw lines of different colours - cv::line(Image,imagePoints[0],imagePoints[1],Scalar(0,0,255,255),1,CV_AA); - cv::line(Image,imagePoints[0],imagePoints[2],Scalar(0,255,0,255),1,CV_AA); - cv::line(Image,imagePoints[0],imagePoints[3],Scalar(255,0,0,255),1,CV_AA); - putText(Image,"x", imagePoints[1],FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0,0,255,255),2); - putText(Image,"y", imagePoints[2],FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0,255,0,255),2); - putText(Image,"z", imagePoints[3],FONT_HERSHEY_SIMPLEX, 0.6, Scalar(255,0,0,255),2); +void CvDrawingUtils::draw3dAxis(cv::Mat &Image, Marker &m, const CameraParameters &CP) { + + float size = m.ssize * 3; + Mat objectPoints(4, 3, CV_32FC1); + objectPoints.at< float >(0, 0) = 0; + objectPoints.at< float >(0, 1) = 0; + objectPoints.at< float >(0, 2) = 0; + objectPoints.at< float >(1, 0) = size; + objectPoints.at< float >(1, 1) = 0; + objectPoints.at< float >(1, 2) = 0; + objectPoints.at< float >(2, 0) = 0; + objectPoints.at< float >(2, 1) = size; + objectPoints.at< float >(2, 2) = 0; + objectPoints.at< float >(3, 0) = 0; + objectPoints.at< float >(3, 1) = 0; + objectPoints.at< float >(3, 2) = size; + + vector< Point2f > imagePoints; + cv::projectPoints(objectPoints, m.Rvec, m.Tvec, CP.CameraMatrix, CP.Distorsion, imagePoints); + // draw lines of different colours + cv::line(Image, imagePoints[0], imagePoints[1], Scalar(0, 0, 255, 255), 1, CV_AA); + cv::line(Image, imagePoints[0], imagePoints[2], Scalar(0, 255, 0, 255), 1, CV_AA); + cv::line(Image, imagePoints[0], imagePoints[3], Scalar(255, 0, 0, 255), 1, CV_AA); + putText(Image, "x", imagePoints[1], FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0, 0, 255, 255), 2); + putText(Image, "y", imagePoints[2], FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0, 255, 0, 255), 2); + putText(Image, "z", imagePoints[3], FONT_HERSHEY_SIMPLEX, 0.6, Scalar(255, 0, 0, 255), 2); } /**** @@ -69,78 +70,75 @@ void CvDrawingUtils::draw3dAxis(cv::Mat &Image,Marker &m,const CameraParameters * * ****/ -void CvDrawingUtils::draw3dCube(cv::Mat &Image,Marker &m,const CameraParameters &CP, bool setYperpendicular) -{ - Mat objectPoints (8,3,CV_32FC1); - double halfSize=m.ssize/2; - - if(setYperpendicular) { - objectPoints.at(0,0)=-halfSize; - objectPoints.at(0,1)=0; - objectPoints.at(0,2)=-halfSize; - objectPoints.at(1,0)=halfSize; - objectPoints.at(1,1)=0; - objectPoints.at(1,2)=-halfSize; - objectPoints.at(2,0)=halfSize; - objectPoints.at(2,1)=0; - objectPoints.at(2,2)=halfSize; - objectPoints.at(3,0)=-halfSize; - objectPoints.at(3,1)=0; - objectPoints.at(3,2)=halfSize; - - objectPoints.at(4,0)=-halfSize; - objectPoints.at(4,1)=m.ssize; - objectPoints.at(4,2)=-halfSize; - objectPoints.at(5,0)=halfSize; - objectPoints.at(5,1)=m.ssize; - objectPoints.at(5,2)=-halfSize; - objectPoints.at(6,0)=halfSize; - objectPoints.at(6,1)=m.ssize; - objectPoints.at(6,2)=halfSize; - objectPoints.at(7,0)=-halfSize; - objectPoints.at(7,1)=m.ssize; - objectPoints.at(7,2)=halfSize; - } - else { - objectPoints.at(0,0)=-halfSize; - objectPoints.at(0,1)=-halfSize; - objectPoints.at(0,2)=0; - objectPoints.at(1,0)=halfSize; - objectPoints.at(1,1)=-halfSize; - objectPoints.at(1,2)=0; - objectPoints.at(2,0)=halfSize; - objectPoints.at(2,1)=halfSize; - objectPoints.at(2,2)=0; - objectPoints.at(3,0)=-halfSize; - objectPoints.at(3,1)=halfSize; - objectPoints.at(3,2)=0; - - objectPoints.at(4,0)=-halfSize; - objectPoints.at(4,1)=-halfSize; - objectPoints.at(4,2)=m.ssize; - objectPoints.at(5,0)=halfSize; - objectPoints.at(5,1)=-halfSize; - objectPoints.at(5,2)=m.ssize; - objectPoints.at(6,0)=halfSize; - objectPoints.at(6,1)=halfSize; - objectPoints.at(6,2)=m.ssize; - objectPoints.at(7,0)=-halfSize; - objectPoints.at(7,1)=halfSize; - objectPoints.at(7,2)=m.ssize; +void CvDrawingUtils::draw3dCube(cv::Mat &Image, Marker &m, const CameraParameters &CP, bool setYperpendicular) { + Mat objectPoints(8, 3, CV_32FC1); + double halfSize = m.ssize / 2; + + if (setYperpendicular) { + objectPoints.at< float >(0, 0) = -halfSize; + objectPoints.at< float >(0, 1) = 0; + objectPoints.at< float >(0, 2) = -halfSize; + objectPoints.at< float >(1, 0) = halfSize; + objectPoints.at< float >(1, 1) = 0; + objectPoints.at< float >(1, 2) = -halfSize; + objectPoints.at< float >(2, 0) = halfSize; + objectPoints.at< float >(2, 1) = 0; + objectPoints.at< float >(2, 2) = halfSize; + objectPoints.at< float >(3, 0) = -halfSize; + objectPoints.at< float >(3, 1) = 0; + objectPoints.at< float >(3, 2) = halfSize; + + objectPoints.at< float >(4, 0) = -halfSize; + objectPoints.at< float >(4, 1) = m.ssize; + objectPoints.at< float >(4, 2) = -halfSize; + objectPoints.at< float >(5, 0) = halfSize; + objectPoints.at< float >(5, 1) = m.ssize; + objectPoints.at< float >(5, 2) = -halfSize; + objectPoints.at< float >(6, 0) = halfSize; + objectPoints.at< float >(6, 1) = m.ssize; + objectPoints.at< float >(6, 2) = halfSize; + objectPoints.at< float >(7, 0) = -halfSize; + objectPoints.at< float >(7, 1) = m.ssize; + objectPoints.at< float >(7, 2) = halfSize; + } else { + objectPoints.at< float >(0, 0) = -halfSize; + objectPoints.at< float >(0, 1) = -halfSize; + objectPoints.at< float >(0, 2) = 0; + objectPoints.at< float >(1, 0) = halfSize; + objectPoints.at< float >(1, 1) = -halfSize; + objectPoints.at< float >(1, 2) = 0; + objectPoints.at< float >(2, 0) = halfSize; + objectPoints.at< float >(2, 1) = halfSize; + objectPoints.at< float >(2, 2) = 0; + objectPoints.at< float >(3, 0) = -halfSize; + objectPoints.at< float >(3, 1) = halfSize; + objectPoints.at< float >(3, 2) = 0; + + objectPoints.at< float >(4, 0) = -halfSize; + objectPoints.at< float >(4, 1) = -halfSize; + objectPoints.at< float >(4, 2) = m.ssize; + objectPoints.at< float >(5, 0) = halfSize; + objectPoints.at< float >(5, 1) = -halfSize; + objectPoints.at< float >(5, 2) = m.ssize; + objectPoints.at< float >(6, 0) = halfSize; + objectPoints.at< float >(6, 1) = halfSize; + objectPoints.at< float >(6, 2) = m.ssize; + objectPoints.at< float >(7, 0) = -halfSize; + objectPoints.at< float >(7, 1) = halfSize; + objectPoints.at< float >(7, 2) = m.ssize; } - vector imagePoints; - projectPoints( objectPoints, m.Rvec,m.Tvec, CP.CameraMatrix,CP.Distorsion, imagePoints); -//draw lines of different colours - for (int i=0;i<4;i++) - cv::line(Image,imagePoints[i],imagePoints[(i+1)%4],Scalar(0,0,255,255),1,CV_AA); + vector< Point2f > imagePoints; + projectPoints(objectPoints, m.Rvec, m.Tvec, CP.CameraMatrix, CP.Distorsion, imagePoints); + // draw lines of different colours + for (int i = 0; i < 4; i++) + cv::line(Image, imagePoints[i], imagePoints[(i + 1) % 4], Scalar(0, 0, 255, 255), 1, CV_AA); - for (int i=0;i<4;i++) - cv::line(Image,imagePoints[i+4],imagePoints[4+(i+1)%4],Scalar(0,0,255,255),1,CV_AA); - - for (int i=0;i<4;i++) - cv::line(Image,imagePoints[i],imagePoints[i+4],Scalar(0,0,255,255),1,CV_AA); + for (int i = 0; i < 4; i++) + cv::line(Image, imagePoints[i + 4], imagePoints[4 + (i + 1) % 4], Scalar(0, 0, 255, 255), 1, CV_AA); + for (int i = 0; i < 4; i++) + cv::line(Image, imagePoints[i], imagePoints[i + 4], Scalar(0, 0, 255, 255), 1, CV_AA); } @@ -149,24 +147,31 @@ void CvDrawingUtils::draw3dCube(cv::Mat &Image,Marker &m,const CameraParameters * * ****/ -void CvDrawingUtils::draw3dAxis(cv::Mat &Image,Board &B,const CameraParameters &CP) -{ -Mat objectPoints (4,3,CV_32FC1); -objectPoints.at(0,0)=0;objectPoints.at(0,1)=0;objectPoints.at(0,2)=0; -objectPoints.at(1,0)=2*B[0].ssize;objectPoints.at(1,1)=0;objectPoints.at(1,2)=0; -objectPoints.at(2,0)=0;objectPoints.at(2,1)=2*B[0].ssize;objectPoints.at(2,2)=0; -objectPoints.at(3,0)=0;objectPoints.at(3,1)=0;objectPoints.at(3,2)=2*B[0].ssize; - -vector imagePoints; -projectPoints( objectPoints, B.Rvec,B.Tvec, CP.CameraMatrix, CP.Distorsion, imagePoints); -//draw lines of different colours -cv::line(Image,imagePoints[0],imagePoints[1],Scalar(0,0,255,255),2,CV_AA); -cv::line(Image,imagePoints[0],imagePoints[2],Scalar(0,255,0,255),2,CV_AA); -cv::line(Image,imagePoints[0],imagePoints[3],Scalar(255,0,0,255),2,CV_AA); - -putText(Image,"X", imagePoints[1],FONT_HERSHEY_SIMPLEX, 1, Scalar(0,0,255,255),2); -putText(Image,"Y", imagePoints[2],FONT_HERSHEY_SIMPLEX, 1, Scalar(0,255,0,255),2); -putText(Image,"Z", imagePoints[3],FONT_HERSHEY_SIMPLEX, 1, Scalar(255,0,0,255),2); +void CvDrawingUtils::draw3dAxis(cv::Mat &Image, Board &B, const CameraParameters &CP) { + Mat objectPoints(4, 3, CV_32FC1); + objectPoints.at< float >(0, 0) = 0; + objectPoints.at< float >(0, 1) = 0; + objectPoints.at< float >(0, 2) = 0; + objectPoints.at< float >(1, 0) = 2 * B[0].ssize; + objectPoints.at< float >(1, 1) = 0; + objectPoints.at< float >(1, 2) = 0; + objectPoints.at< float >(2, 0) = 0; + objectPoints.at< float >(2, 1) = 2 * B[0].ssize; + objectPoints.at< float >(2, 2) = 0; + objectPoints.at< float >(3, 0) = 0; + objectPoints.at< float >(3, 1) = 0; + objectPoints.at< float >(3, 2) = 2 * B[0].ssize; + + vector< Point2f > imagePoints; + projectPoints(objectPoints, B.Rvec, B.Tvec, CP.CameraMatrix, CP.Distorsion, imagePoints); + // draw lines of different colours + cv::line(Image, imagePoints[0], imagePoints[1], Scalar(0, 0, 255, 255), 2, CV_AA); + cv::line(Image, imagePoints[0], imagePoints[2], Scalar(0, 255, 0, 255), 2, CV_AA); + cv::line(Image, imagePoints[0], imagePoints[3], Scalar(255, 0, 0, 255), 2, CV_AA); + + putText(Image, "X", imagePoints[1], FONT_HERSHEY_SIMPLEX, 1, Scalar(0, 0, 255, 255), 2); + putText(Image, "Y", imagePoints[2], FONT_HERSHEY_SIMPLEX, 1, Scalar(0, 255, 0, 255), 2); + putText(Image, "Z", imagePoints[3], FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 0, 0, 255), 2); } @@ -175,47 +180,76 @@ putText(Image,"Z", imagePoints[3],FONT_HERSHEY_SIMPLEX, 1, Scalar(255,0,0,255),2 * * ****/ -void CvDrawingUtils::draw3dCube(cv::Mat &Image,Board &B,const CameraParameters &CP, bool setYperpendicular) -{ - -float cubeSize=B[0].ssize; -float txz=-cubeSize/2; -Mat objectPoints (8,3,CV_32FC1); - -if(setYperpendicular) { - objectPoints.at(0,0)=txz;objectPoints.at(0,1)=0;objectPoints.at(0,2)=txz; - objectPoints.at(1,0)=txz+cubeSize;objectPoints.at(1,1)=0;objectPoints.at(1,2)=txz; - objectPoints.at(2,0)=txz+cubeSize;objectPoints.at(2,1)=cubeSize;objectPoints.at(2,2)=txz; - objectPoints.at(3,0)=txz;objectPoints.at(3,1)=cubeSize;objectPoints.at(3,2)=txz; - - objectPoints.at(4,0)=txz;objectPoints.at(4,1)=0;objectPoints.at(4,2)=txz+cubeSize; - objectPoints.at(5,0)=txz+cubeSize;objectPoints.at(5,1)=0;objectPoints.at(5,2)=txz+cubeSize; - objectPoints.at(6,0)=txz+cubeSize;objectPoints.at(6,1)=cubeSize;objectPoints.at(6,2)=txz+cubeSize; - objectPoints.at(7,0)=txz;objectPoints.at(7,1)=cubeSize;objectPoints.at(7,2)=txz+cubeSize; -} -else { - objectPoints.at(0,0)=txz;objectPoints.at(0,2)=0;objectPoints.at(0,1)=txz; - objectPoints.at(1,0)=txz+cubeSize;objectPoints.at(1,2)=0;objectPoints.at(1,1)=txz; - objectPoints.at(2,0)=txz+cubeSize;objectPoints.at(2,2)=-cubeSize;objectPoints.at(2,1)=txz; - objectPoints.at(3,0)=txz;objectPoints.at(3,2)=-cubeSize;objectPoints.at(3,1)=txz; - - objectPoints.at(4,0)=txz;objectPoints.at(4,2)=0;objectPoints.at(4,1)=txz+cubeSize; - objectPoints.at(5,0)=txz+cubeSize;objectPoints.at(5,2)=0;objectPoints.at(5,1)=txz+cubeSize; - objectPoints.at(6,0)=txz+cubeSize;objectPoints.at(6,2)=-cubeSize;objectPoints.at(6,1)=txz+cubeSize; - objectPoints.at(7,0)=txz;objectPoints.at(7,2)=-cubeSize;objectPoints.at(7,1)=txz+cubeSize; -} +void CvDrawingUtils::draw3dCube(cv::Mat &Image, Board &B, const CameraParameters &CP, bool setYperpendicular) { + + float cubeSize = B[0].ssize; + float txz = -cubeSize / 2; + Mat objectPoints(8, 3, CV_32FC1); + + if (setYperpendicular) { + objectPoints.at< float >(0, 0) = txz; + objectPoints.at< float >(0, 1) = 0; + objectPoints.at< float >(0, 2) = txz; + objectPoints.at< float >(1, 0) = txz + cubeSize; + objectPoints.at< float >(1, 1) = 0; + objectPoints.at< float >(1, 2) = txz; + objectPoints.at< float >(2, 0) = txz + cubeSize; + objectPoints.at< float >(2, 1) = cubeSize; + objectPoints.at< float >(2, 2) = txz; + objectPoints.at< float >(3, 0) = txz; + objectPoints.at< float >(3, 1) = cubeSize; + objectPoints.at< float >(3, 2) = txz; + + objectPoints.at< float >(4, 0) = txz; + objectPoints.at< float >(4, 1) = 0; + objectPoints.at< float >(4, 2) = txz + cubeSize; + objectPoints.at< float >(5, 0) = txz + cubeSize; + objectPoints.at< float >(5, 1) = 0; + objectPoints.at< float >(5, 2) = txz + cubeSize; + objectPoints.at< float >(6, 0) = txz + cubeSize; + objectPoints.at< float >(6, 1) = cubeSize; + objectPoints.at< float >(6, 2) = txz + cubeSize; + objectPoints.at< float >(7, 0) = txz; + objectPoints.at< float >(7, 1) = cubeSize; + objectPoints.at< float >(7, 2) = txz + cubeSize; + } else { + objectPoints.at< float >(0, 0) = txz; + objectPoints.at< float >(0, 2) = 0; + objectPoints.at< float >(0, 1) = txz; + objectPoints.at< float >(1, 0) = txz + cubeSize; + objectPoints.at< float >(1, 2) = 0; + objectPoints.at< float >(1, 1) = txz; + objectPoints.at< float >(2, 0) = txz + cubeSize; + objectPoints.at< float >(2, 2) = -cubeSize; + objectPoints.at< float >(2, 1) = txz; + objectPoints.at< float >(3, 0) = txz; + objectPoints.at< float >(3, 2) = -cubeSize; + objectPoints.at< float >(3, 1) = txz; + + objectPoints.at< float >(4, 0) = txz; + objectPoints.at< float >(4, 2) = 0; + objectPoints.at< float >(4, 1) = txz + cubeSize; + objectPoints.at< float >(5, 0) = txz + cubeSize; + objectPoints.at< float >(5, 2) = 0; + objectPoints.at< float >(5, 1) = txz + cubeSize; + objectPoints.at< float >(6, 0) = txz + cubeSize; + objectPoints.at< float >(6, 2) = -cubeSize; + objectPoints.at< float >(6, 1) = txz + cubeSize; + objectPoints.at< float >(7, 0) = txz; + objectPoints.at< float >(7, 2) = -cubeSize; + objectPoints.at< float >(7, 1) = txz + cubeSize; + } -vector imagePoints; -projectPoints( objectPoints,B.Rvec,B.Tvec, CP.CameraMatrix, CP.Distorsion, imagePoints); -//draw lines of different colours -for(int i=0;i<4;i++) - cv::line(Image,imagePoints[i],imagePoints[(i+1)%4],Scalar(0,0,255,255),1,CV_AA); + vector< Point2f > imagePoints; + projectPoints(objectPoints, B.Rvec, B.Tvec, CP.CameraMatrix, CP.Distorsion, imagePoints); + // draw lines of different colours + for (int i = 0; i < 4; i++) + cv::line(Image, imagePoints[i], imagePoints[(i + 1) % 4], Scalar(0, 0, 255, 255), 1, CV_AA); -for(int i=0;i<4;i++) - cv::line(Image,imagePoints[i+4],imagePoints[4+(i+1)%4],Scalar(0,0,255,255),1,CV_AA); + for (int i = 0; i < 4; i++) + cv::line(Image, imagePoints[i + 4], imagePoints[4 + (i + 1) % 4], Scalar(0, 0, 255, 255), 1, CV_AA); -for(int i=0;i<4;i++) - cv::line(Image,imagePoints[i],imagePoints[i+4],Scalar(0,0,255,255),1,CV_AA); + for (int i = 0; i < 4; i++) + cv::line(Image, imagePoints[i], imagePoints[i + 4], Scalar(0, 0, 255, 255), 1, CV_AA); } - } diff --git a/src/cvdrawingutils.h b/src/cvdrawingutils.h index b8eff34..a7012ae 100644 --- a/src/cvdrawingutils.h +++ b/src/cvdrawingutils.h @@ -29,24 +29,19 @@ or implied, of Rafael Muñoz Salinas. #define _ArUco_DrawUtils_H_ #include "exports.h" #include "aruco.h" -namespace aruco -{ - /**\brief A set of functions to draw in opencv images - */ - class ARUCO_EXPORTS CvDrawingUtils - { +namespace aruco { +/**\brief A set of functions to draw in opencv images + */ +class ARUCO_EXPORTS CvDrawingUtils { public: - - static void draw3dAxis(cv::Mat &Image,Marker &m,const CameraParameters &CP); - - static void draw3dCube(cv::Mat &Image,Marker &m,const CameraParameters &CP, bool setYperpendicular=false); - - static void draw3dAxis(cv::Mat &Image,Board &m,const CameraParameters &CP); - - static void draw3dCube(cv::Mat &Image,Board &m,const CameraParameters &CP, bool setYperpendicular=false); - - }; + static void draw3dAxis(cv::Mat &Image, Marker &m, const CameraParameters &CP); + + static void draw3dCube(cv::Mat &Image, Marker &m, const CameraParameters &CP, bool setYperpendicular = false); + + static void draw3dAxis(cv::Mat &Image, Board &m, const CameraParameters &CP); + + static void draw3dCube(cv::Mat &Image, Board &m, const CameraParameters &CP, bool setYperpendicular = false); +}; }; #endif - diff --git a/src/exports.h b/src/exports.h index 2b87960..d189d9f 100644 --- a/src/exports.h +++ b/src/exports.h @@ -25,22 +25,22 @@ The views and conclusions contained in the software and documentation are those authors and should not be interpreted as representing official policies, either expressed or implied, of Rafael Muñoz Salinas. ********************************/ - - - -#ifndef __OPENARUCO_CORE_TYPES_H__ -#define __OPENARUCO_CORE_TYPES_H__ - -#if !defined _CRT_SECURE_NO_DEPRECATE && _MSC_VER > 1300 -#define _CRT_SECURE_NO_DEPRECATE /* to avoid multiple Visual Studio 2005 warnings */ -#endif - - -#if (defined WIN32 || defined _WIN32 || defined WINCE) && defined DSO_EXPORTS - #define ARUCO_EXPORTS __declspec(dllexport) -#else - #define ARUCO_EXPORTS -#endif - - -#endif + + + +#ifndef __OPENARUCO_CORE_TYPES_H__ +#define __OPENARUCO_CORE_TYPES_H__ + +#if !defined _CRT_SECURE_NO_DEPRECATE && _MSC_VER > 1300 +#define _CRT_SECURE_NO_DEPRECATE /* to avoid multiple Visual Studio 2005 warnings */ +#endif + + +#if (defined WIN32 || defined _WIN32 || defined WINCE) && defined DSO_EXPORTS +#define ARUCO_EXPORTS __declspec(dllexport) +#else +#define ARUCO_EXPORTS +#endif + + +#endif diff --git a/src/highlyreliablemarkers.cpp b/src/highlyreliablemarkers.cpp index a50f939..4f9f343 100755 --- a/src/highlyreliablemarkers.cpp +++ b/src/highlyreliablemarkers.cpp @@ -27,454 +27,475 @@ or implied, of Rafael Muñoz Salinas. ********************************/ #include "highlyreliablemarkers.h" +#include +using namespace std; namespace aruco { - // static variables from HighlyReliableMarkers. Need to be here to avoid linking errors - Dictionary HighlyReliableMarkers::_D; - HighlyReliableMarkers::BalancedBinaryTree HighlyReliableMarkers::_binaryTree; - unsigned int HighlyReliableMarkers::_n, HighlyReliableMarkers::_ncellsBorder, HighlyReliableMarkers::_correctionDistance; - int HighlyReliableMarkers::_swidth; +// static variables from HighlyReliableMarkers. Need to be here to avoid linking errors +Dictionary HighlyReliableMarkers::_D; +HighlyReliableMarkers::BalancedBinaryTree HighlyReliableMarkers::_binaryTree; +unsigned int HighlyReliableMarkers::_n, HighlyReliableMarkers::_ncellsBorder, HighlyReliableMarkers::_correctionDistance; +int HighlyReliableMarkers::_swidth; - /** - */ - MarkerCode::MarkerCode(unsigned int n) { +/** +*/ +MarkerCode::MarkerCode(unsigned int n) { // resize bits vectors and initialize to 0 - for(unsigned int i=0; i<4; i++) { - _bits[i].resize(n*n); - for(unsigned int j=0; j<_bits[i].size(); j++) _bits[i][j]=0; - _ids[i] = 0; // ids are also 0 + for (unsigned int i = 0; i < 4; i++) { + _bits[i].resize(n * n); + for (unsigned int j = 0; j < _bits[i].size(); j++) + _bits[i][j] = 0; + _ids[i] = 0; // ids are also 0 } _n = n; - }; - - - /** - */ - MarkerCode::MarkerCode(const MarkerCode& MC) - { - for(unsigned int i=0; i<4; i++) { - _bits[i] = MC._bits[i]; - _ids[i] = MC._ids[i]; +}; + + +/** + */ +MarkerCode::MarkerCode(const MarkerCode &MC) { + for (unsigned int i = 0; i < 4; i++) { + _bits[i] = MC._bits[i]; + _ids[i] = MC._ids[i]; } _n = MC._n; - } +} + + - - - /** - */ - void MarkerCode::set(unsigned int pos, bool val) { +/** + */ +void MarkerCode::set(unsigned int pos, bool val, bool updateIds) { // if not the same value - if( get(pos) != val ) { - for(unsigned int i=0; i<4; i++) { // calculate bit coordinates for each rotation - unsigned int y=pos/n(), x=pos%n(); // if rotation 0, dont do anything - // else calculate bit position in that rotation - if(i==1) { unsigned int aux=y; y=x; x=n()-aux-1; } - else if(i==2) { y=n()-y-1; x=n()-x-1; } - else if(i==3) { unsigned int aux=y; y=n()-x-1; x=aux; } - unsigned int rotPos = y*n()+x; // calculate position in the unidimensional string - _bits[i][rotPos] = val; // modify value - // update identifier in that rotation - if(val==true) _ids[i] += (unsigned int)pow(float(2),float(rotPos)); // if 1, add 2^pos - else _ids[i] -= (unsigned int)pow(float(2),float(rotPos)); // if 0, substract 2^pos - } + if (get(pos) != val) { + for (unsigned int i = 0; i < 4; i++) { // calculate bit coordinates for each rotation + unsigned int y = pos / n(), x = pos % n(); // if rotation 0, dont do anything + // else calculate bit position in that rotation + if (i == 1) { + unsigned int aux = y; + y = x; + x = n() - aux - 1; + } else if (i == 2) { + y = n() - y - 1; + x = n() - x - 1; + } else if (i == 3) { + unsigned int aux = y; + y = n() - x - 1; + x = aux; + } + unsigned int rotPos = y * n() + x; // calculate position in the unidimensional string + _bits[i][rotPos] = val; // modify value + // update identifier in that rotation + if(updateIds) { + if (val == true) + _ids[i] += (unsigned int)pow(float(2), float(rotPos)); // if 1, add 2^pos + else + _ids[i] -= (unsigned int)pow(float(2), float(rotPos)); // if 0, substract 2^pos + } + } } - } - - - /** - */ - unsigned int MarkerCode::selfDistance(unsigned int &minRot) { - unsigned int res = _bits[0].size(); // init to n*n (max value) - for(unsigned int i=1; i<4; i++) { // self distance is not calculated for rotation 0 - unsigned int hammdist = hammingDistance(_bits[0], _bits[i]); - if(hammdist((i+borderSize)*cellSize+k, (j+borderSize)*cellSize+l) = 255; - } - } - } - } + for (unsigned int i = 0; i < n(); i++) { + for (unsigned int j = 0; j < n(); j++) { + if (_bits[0][i * n() + j] != 0) { // just draw if it is 1, since the image has been init to 0 + // double for to go over all the pixels in the cell + for (unsigned int k = 0; k < cellSize; k++) { + for (unsigned int l = 0; l < cellSize; l++) { + img.at< uchar >((i + borderSize) * cellSize + k, (j + borderSize) * cellSize + l) = 255; + } + } + } + } } - return img; - } - - - /** - */ - unsigned int MarkerCode::hammingDistance(std::vector m1, std::vector m2) { - unsigned int res=0; - for(unsigned int i=0; i &m1, const std::vector< bool > &m2) const { + unsigned int res = 0; + for (unsigned int i = 0; i < m1.size(); i++) + if (m1[i] != m2[i]) + res++; return res; - }; - - - - - - - /** - */ - bool Dictionary::fromFile(std::string filename) { +} + + + + +/** +*/ +bool Dictionary::fromFile(std::string filename) { cv::FileStorage fs(filename, cv::FileStorage::READ); int nmarkers, markersize; - + // read number of markers - fs["nmarkers"] >> nmarkers; // cardinal of D - fs["markersize"] >> markersize; // n - + fs["nmarkers"] >> nmarkers; // cardinal of D + fs["markersize"] >> markersize; // n + fs["tau0"] >> tau0; + // read each marker info - for (int i=0; i> s; - MarkerCode m(markersize); - m.fromString(s); - push_back(m); + MarkerCode m(markersize); + m.fromString(s); + push_back(m); } - fs.release(); + fs.release(); return true; - }; - - /** - */ - bool Dictionary::toFile(std::string filename) { +}; + +/** + */ +bool Dictionary::toFile(std::string filename) { cv::FileStorage fs(filename, cv::FileStorage::WRITE); // save number of markers - fs << "nmarkers" << (int)size(); // cardinal of D - fs << "markersize" << (int)( (*this)[0].n() ); // n + fs << "nmarkers" << (int)size(); // cardinal of D + fs << "markersize" << (int)((*this)[0].n()); // n + fs << "tau0" << (int)(this->tau0); // n // save each marker code - for (unsigned int i=0; i getRotation(unsigned int rot) { return _bits[rot]; }; - - /** - * Set the value of a vit in a specific rotation - * The marker is refered as a unidimensional string of bits, i.e. pos=y*n+x - * This method assure consistency of the marker code: - * - The rest of rotations are updated automatically when performing a modification - * - The id values in all rotations are automatically updated too - * This is the only method to modify a bit value - */ - void set(unsigned int pos, bool val); - - /** - * Return the full size of the marker (n*n) - */ - unsigned int size() {return n()*n(); }; - - /** - * Return the value of marker dimension (n) - */ - unsigned int n() {return _n; }; - - /** - * Return the self distance S(m) of the marker (Equation 8) - * Assign to minRot the rotation of minimun hamming distance - */ - unsigned int selfDistance(unsigned int &minRot); - - /** - * Return the self distance S(m) of the marker (Equation 8) - * Same method as selfDistance(uint &minRot), except this doesnt return minRot value. - */ - unsigned int selfDistance() { - unsigned int minRot; - return selfDistance(minRot); - }; - - /** - * Return the rotation invariant distance to another marker, D(m1, m2) (Equation 6) - * Assign to minRot the rotation of minimun hamming distance. The rotation refers to the marker passed as parameter, m - */ - unsigned int distance(MarkerCode m, unsigned int &minRot); - - /** - * Return the rotation invariant distance to another marker, D(m1, m2) (Equation 6) - * Same method as distance(MarkerCode m, uint &minRot), except this doesnt return minRot value. - */ - unsigned int distance(MarkerCode m) { - unsigned int minRot; - return distance(m, minRot); - }; - - /** - * Read marker bits from a string of "0"s and "1"s - */ - void fromString(std::string s); - - /** - * Convert marker to a string of "0"s and "1"s - */ - std::string toString(); - - - /** - * Convert marker to a cv::Mat image of (pixSize x pixSize) pixels - * It adds a black border of one cell size - */ - cv::Mat getImg(unsigned int pixSize); - -private: - unsigned int _ids[4]; // ids in the four rotations - std::vector _bits[4]; // bit strings in the four rotations - unsigned int _n; // marker dimension - - /** - * Return hamming distance between two bit vectors - */ - unsigned int hammingDistance(std::vector m1, std::vector m2); - + public: + /** + * Constructor, receive dimension of marker + */ + MarkerCode(unsigned int n = 0); + + /** + * Copy Constructor + */ + MarkerCode(const MarkerCode &MC); + + /** + * Get id of a specific rotation as the number obtaiend from the concatenation of all the bits + */ + unsigned int getId(unsigned int rot = 0) const { return _ids[rot]; }; + + /** + * Get a bit value in a specific rotation. + * The marker is refered as a unidimensional string of bits, i.e. pos=y*n+x + */ + bool get(unsigned int pos, unsigned int rot = 0) const { return _bits[rot][pos]; } + + /** + * Get the string of bits for a specific rotation + */ + const std::vector< bool > &getRotation(unsigned int rot) const { return _bits[rot]; }; + + /** + * Set the value of a bit in a specific rotation + * The marker is refered as a unidimensional string of bits, i.e. pos=y*n+x + * This method assure consistency of the marker code: + * - The rest of rotations are updated automatically when performing a modification + * - The id values in all rotations are automatically updated too + * This is the only method to modify a bit value + */ + void set(unsigned int pos, bool val, bool updateIds=true); + + /** + * Return the full size of the marker (n*n) + */ + unsigned int size() const { return n() * n(); }; + + /** + * Return the value of marker dimension (n) + */ + unsigned int n() const { return _n; }; + + /** + * Return the self distance S(m) of the marker (Equation 8) + * Assign to minRot the rotation of minimun hamming distance + */ + unsigned int selfDistance(unsigned int &minRot) const; + + /** + * Return the self distance S(m) of the marker (Equation 8) + * Same method as selfDistance(uint &minRot), except this doesnt return minRot value. + */ + unsigned int selfDistance() const { + unsigned int minRot; + return selfDistance(minRot); + }; + + /** + * Return the rotation invariant distance to another marker, D(m1, m2) (Equation 6) + * Assign to minRot the rotation of minimun hamming distance. The rotation refers to the marker passed as parameter, m + */ + unsigned int distance(const MarkerCode &m, unsigned int &minRot) const; + + /** + * Return the rotation invariant distance to another marker, D(m1, m2) (Equation 6) + * Same method as distance(MarkerCode m, uint &minRot), except this doesnt return minRot value. + */ + unsigned int distance(const MarkerCode &m) const { + unsigned int minRot; + return distance(m, minRot); + }; + + /** + * Read marker bits from a string of "0"s and "1"s + */ + void fromString(std::string s); + + /** + * Convert marker to a string of "0"s and "1"s + */ + std::string toString() const; + + + /** + * Convert marker to a cv::Mat image of (pixSize x pixSize) pixels + * It adds a black border of one cell size + */ + cv::Mat getImg(unsigned int pixSize) const; + + private: + unsigned int _ids[4]; // ids in the four rotations + std::vector< bool > _bits[4]; // bit strings in the four rotations + unsigned int _n; // marker dimension + + /** + * Return hamming distance between two bit vectors + */ + unsigned int hammingDistance(const std::vector< bool > &m1, const std::vector< bool > &m2) const; }; /** * This class represent a marker dictionary as a vector of MarkerCodes - * - * + * + * */ -class ARUCO_EXPORTS Dictionary : public std::vector { -public: - - /** - * Read dictionary from a .yml opencv file - */ - bool fromFile(std::string filename); - - /** - * Write dictionary to a .yml opencv file - */ - bool toFile(std::string filename); - - /** - * Return the distance of a marker to the dictionary, D(m,D) (Equation 7) - * Assign to minMarker the marker index in the dictionary with minimun distance to m - * Assign to minRot the rotation of minimun hamming distance. The rotation refers to the marker passed as parameter, m - */ - unsigned int distance(MarkerCode m, unsigned int &minMarker, unsigned int &minRot); - - /** - * Return the distance of a marker to the dictionary, D(m,D) (Equation 7) - * Same method as distance(MarkerCode m, uint &minMarker, uint &minRot), except this doesnt return minMarker and minRot values. - */ - unsigned int distance(MarkerCode m) { - unsigned int minMarker, minRot; - return distance(m,minMarker,minRot); - } - - /** - * Calculate the minimun distance between the markers in the dictionary (Equation 9) - */ - unsigned int minimunDistance(); - -private: - - // convert to string - template static std::string toStr(T num) { - std::stringstream ss; - ss << num; - return ss.str(); - } - - +class ARUCO_EXPORTS Dictionary : public std::vector< MarkerCode > { + public: + /** + * Read dictionary from a .yml opencv file + */ + bool fromFile(std::string filename); + + /** + * Write dictionary to a .yml opencv file + */ + bool toFile(std::string filename); + + /** + * Return the distance of a marker to the dictionary, D(m,D) (Equation 7) + * Assign to minMarker the marker index in the dictionary with minimun distance to m + * Assign to minRot the rotation of minimun hamming distance. The rotation refers to the marker passed as parameter, m + */ + unsigned int distance(const MarkerCode &m, unsigned int &minMarker, unsigned int &minRot); + + /** + * Return the distance of a marker to the dictionary, D(m,D) (Equation 7) + * Same method as distance(MarkerCode m, uint &minMarker, uint &minRot), except this doesnt return minMarker and minRot values. + */ + unsigned int distance(const MarkerCode &m) { + unsigned int minMarker, minRot; + return distance(m, minMarker, minRot); + } + + /** + * Calculate the minimun distance between the markers in the dictionary (Equation 9) + */ + unsigned int minimunDistance(); + + int tau0; + + private: + // convert to string + template < class T > static std::string toStr(T num) { + std::stringstream ss; + ss << num; + return ss.str(); + } }; /** * Highly Reliable Marker Detector Class - * - * + * + * */ -class ARUCO_EXPORTS HighlyReliableMarkers -{ -public: - - /** - * Balanced Binary Tree for a marker dictionary - * - */ - class BalancedBinaryTree { - +class ARUCO_EXPORTS HighlyReliableMarkers { public: - /** - * Create the tree for dictionary D + * Balanced Binary Tree for a marker dictionary + * */ - void loadDictionary(Dictionary *D); - + class BalancedBinaryTree { + + public: + /** + * Create the tree for dictionary D + */ + void loadDictionary(Dictionary *D); + + /** + * Search a id in the dictionary. Return true if found, false otherwise. + */ + bool findId(unsigned int id, unsigned int &orgPos); + + private: + std::vector< std::pair< unsigned int, unsigned int > > _orderD; // dictionary sorted by id, + // first element is the id, + // second element is the position in original D + std::vector< std::pair< int, int > > _binaryTree; // binary tree itself (as a vector), each element is a node of the tree + // first element indicate the position in _binaryTree of the lower child + // second element is the position in _binaryTree of the higher child + // -1 value indicates no lower or higher child + unsigned int _root; // position in _binaryTree of the root node of the tree + }; + /** - * Search a id in the dictionary. Return true if found, false otherwise. - */ - bool findId(unsigned int id, unsigned int &orgPos); - + * Load the dictionary that will be detected or read it directly from file + */ + // correctionDistance [0,1] 0: totalmente restrictivo, 1 mas flexible + static bool loadDictionary(Dictionary D, float correctionDistance = 1); + static bool loadDictionary(std::string filename, float correctionDistance = 1); + static Dictionary &getDictionary() { return _D; } + + + /** + * Detect marker in a canonical image. Perform detection and error correction + * Return marker id in 0 rotation, or -1 if not found + * Assign the detected rotation of the marker to nRotation + */ + static int detect(const cv::Mat &in, int &nRotations); + + private: + static Dictionary _D; // loaded dictionary + static BalancedBinaryTree _binaryTree; + // marker dimension, marker dimension with borders, maximunCorrectionDistance + static unsigned int _n; + static unsigned int _ncellsBorder; + static unsigned int _correctionDistance; + static int _swidth; // cell size in the canonical image - std::vector< std::pair > _orderD; // dictionary sorted by id, - // first element is the id, - // second element is the position in original D - std::vector< std::pair > _binaryTree; // binary tree itself (as a vector), each element is a node of the tree - // first element indicate the position in _binaryTree of the lower child - // second element is the position in _binaryTree of the higher child - // -1 value indicates no lower or higher child - unsigned int _root; // position in _binaryTree of the root node of the tree - - }; - - /** - * Load the dictionary that will be detected or read it directly from file - */ - static bool loadDictionary(Dictionary D); - static bool loadDictionary(std::string filename); - static Dictionary& getDictionary() { return _D; } - - - /** - * Detect marker in a canonical image. Perform detection and error correction - * Return marker id in 0 rotation, or -1 if not found - * Assign the detected rotation of the marker to nRotation - */ - static int detect(const cv::Mat& in, int& nRotations); - - -private: - static Dictionary _D; // loaded dictionary - static BalancedBinaryTree _binaryTree; - // marker dimension, marker dimension with borders, maximunCorrectionDistance - static unsigned int _n; - static unsigned int _ncellsBorder; - static unsigned int _correctionDistance; - static int _swidth; // cell size in the canonical image - - - /** - * Check marker borders cell in the canonical image are black - */ - static bool checkBorders(cv::Mat grey); - - /** - * Return binary MarkerCode from a canonical image, it ignores borders - */ - static MarkerCode getMarkerCode(cv::Mat grey); - - -}; + /** + * Check marker borders cell in the canonical image are black + */ + static bool checkBorders(cv::Mat grey); + + /** + * Return binary MarkerCode from a canonical image, it ignores borders + */ + static MarkerCode getMarkerCode(const cv::Mat &grey); }; +} + #endif // HIGHLYRELIABLEMARKERS_H diff --git a/src/marker.cpp b/src/marker.cpp index 688f2ab..20523f2 100644 --- a/src/marker.cpp +++ b/src/marker.cpp @@ -31,96 +31,93 @@ or implied, of Rafael Muñoz Salinas. #include #include #include +#include using namespace cv; namespace aruco { /** * */ -Marker::Marker() -{ - id=-1; - ssize=-1; - Rvec.create(3,1,CV_32FC1); - Tvec.create(3,1,CV_32FC1); - for (int i=0;i<3;i++) - Tvec.at(i,0)=Rvec.at(i,0)=-999999; +Marker::Marker() { + id = -1; + ssize = -1; + Rvec.create(3, 1, CV_32FC1); + Tvec.create(3, 1, CV_32FC1); + for (int i = 0; i < 3; i++) + Tvec.at< float >(i, 0) = Rvec.at< float >(i, 0) = -999999; } /** * */ -Marker::Marker(const Marker &M):std::vector(M) -{ +Marker::Marker(const Marker &M) : std::vector< cv::Point2f >(M) { M.Rvec.copyTo(Rvec); M.Tvec.copyTo(Tvec); - id=M.id; - ssize=M.ssize; + id = M.id; + ssize = M.ssize; } /** * */ -Marker::Marker(const std::vector &corners,int _id):std::vector(corners) -{ - id=_id; - ssize=-1; - Rvec.create(3,1,CV_32FC1); - Tvec.create(3,1,CV_32FC1); - for (int i=0;i<3;i++) - Tvec.at(i,0)=Rvec.at(i,0)=-999999; +Marker::Marker(const std::vector< cv::Point2f > &corners, int _id) : std::vector< cv::Point2f >(corners) { + id = _id; + ssize = -1; + Rvec.create(3, 1, CV_32FC1); + Tvec.create(3, 1, CV_32FC1); + for (int i = 0; i < 3; i++) + Tvec.at< float >(i, 0) = Rvec.at< float >(i, 0) = -999999; } /** * */ -void Marker::glGetModelViewMatrix( double modelview_matrix[16])throw(cv::Exception) -{ - //check if paremeters are valid - bool invalid=false; - for (int i=0;i<3 && !invalid ;i++) - { - if (Tvec.at(i,0)!=-999999) invalid|=false; - if (Rvec.at(i,0)!=-999999) invalid|=false; +void Marker::glGetModelViewMatrix(double modelview_matrix[16]) throw(cv::Exception) { + // check if paremeters are valid + bool invalid = false; + for (int i = 0; i < 3 && !invalid; i++) { + if (Tvec.at< float >(i, 0) != -999999) + invalid |= false; + if (Rvec.at< float >(i, 0) != -999999) + invalid |= false; } - if (invalid) throw cv::Exception(9003,"extrinsic parameters are not set","Marker::getModelViewMatrix",__FILE__,__LINE__); - Mat Rot(3,3,CV_32FC1),Jacob; + if (invalid) + throw cv::Exception(9003, "extrinsic parameters are not set", "Marker::getModelViewMatrix", __FILE__, __LINE__); + Mat Rot(3, 3, CV_32FC1), Jacob; Rodrigues(Rvec, Rot, Jacob); double para[3][4]; - for (int i=0;i<3;i++) - for (int j=0;j<3;j++) para[i][j]=Rot.at(i,j); - //now, add the translation - para[0][3]=Tvec.at(0,0); - para[1][3]=Tvec.at(1,0); - para[2][3]=Tvec.at(2,0); - double scale=1; - - modelview_matrix[0 + 0*4] = para[0][0]; + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + para[i][j] = Rot.at< float >(i, j); + // now, add the translation + para[0][3] = Tvec.at< float >(0, 0); + para[1][3] = Tvec.at< float >(1, 0); + para[2][3] = Tvec.at< float >(2, 0); + double scale = 1; + + modelview_matrix[0 + 0 * 4] = para[0][0]; // R1C2 - modelview_matrix[0 + 1*4] = para[0][1]; - modelview_matrix[0 + 2*4] = para[0][2]; - modelview_matrix[0 + 3*4] = para[0][3]; + modelview_matrix[0 + 1 * 4] = para[0][1]; + modelview_matrix[0 + 2 * 4] = para[0][2]; + modelview_matrix[0 + 3 * 4] = para[0][3]; // R2 - modelview_matrix[1 + 0*4] = para[1][0]; - modelview_matrix[1 + 1*4] = para[1][1]; - modelview_matrix[1 + 2*4] = para[1][2]; - modelview_matrix[1 + 3*4] = para[1][3]; + modelview_matrix[1 + 0 * 4] = para[1][0]; + modelview_matrix[1 + 1 * 4] = para[1][1]; + modelview_matrix[1 + 2 * 4] = para[1][2]; + modelview_matrix[1 + 3 * 4] = para[1][3]; // R3 - modelview_matrix[2 + 0*4] = -para[2][0]; - modelview_matrix[2 + 1*4] = -para[2][1]; - modelview_matrix[2 + 2*4] = -para[2][2]; - modelview_matrix[2 + 3*4] = -para[2][3]; - modelview_matrix[3 + 0*4] = 0.0; - modelview_matrix[3 + 1*4] = 0.0; - modelview_matrix[3 + 2*4] = 0.0; - modelview_matrix[3 + 3*4] = 1.0; - if (scale != 0.0) - { + modelview_matrix[2 + 0 * 4] = -para[2][0]; + modelview_matrix[2 + 1 * 4] = -para[2][1]; + modelview_matrix[2 + 2 * 4] = -para[2][2]; + modelview_matrix[2 + 3 * 4] = -para[2][3]; + modelview_matrix[3 + 0 * 4] = 0.0; + modelview_matrix[3 + 1 * 4] = 0.0; + modelview_matrix[3 + 2 * 4] = 0.0; + modelview_matrix[3 + 3 * 4] = 1.0; + if (scale != 0.0) { modelview_matrix[12] *= scale; modelview_matrix[13] *= scale; modelview_matrix[14] *= scale; } - - } @@ -128,41 +125,42 @@ void Marker::glGetModelViewMatrix( double modelview_matrix[16])throw(cv::Excep /**** * */ -void Marker::OgreGetPoseParameters(double position[3], double orientation[4]) throw(cv::Exception) -{ - - //check if paremeters are valid - bool invalid=false; - for (int i=0;i<3 && !invalid ;i++) - { - if (Tvec.at(i,0)!=-999999) invalid|=false; - if (Rvec.at(i,0)!=-999999) invalid|=false; +void Marker::OgreGetPoseParameters(double position[3], double orientation[4]) throw(cv::Exception) { + + // check if paremeters are valid + bool invalid = false; + for (int i = 0; i < 3 && !invalid; i++) { + if (Tvec.at< float >(i, 0) != -999999) + invalid |= false; + if (Rvec.at< float >(i, 0) != -999999) + invalid |= false; } - if (invalid) throw cv::Exception(9003,"extrinsic parameters are not set","Marker::getModelViewMatrix",__FILE__,__LINE__); + if (invalid) + throw cv::Exception(9003, "extrinsic parameters are not set", "Marker::getModelViewMatrix", __FILE__, __LINE__); // calculate position vector - position[0] = -Tvec.ptr(0)[0]; - position[1] = -Tvec.ptr(0)[1]; - position[2] = +Tvec.ptr(0)[2]; + position[0] = -Tvec.ptr< float >(0)[0]; + position[1] = -Tvec.ptr< float >(0)[1]; + position[2] = +Tvec.ptr< float >(0)[2]; // now calculare orientation quaternion - cv::Mat Rot(3,3,CV_32FC1); + cv::Mat Rot(3, 3, CV_32FC1); cv::Rodrigues(Rvec, Rot); // calculate axes for quaternion double stAxes[3][3]; // x axis - stAxes[0][0] = -Rot.at(0,0); - stAxes[0][1] = -Rot.at(1,0); - stAxes[0][2] = +Rot.at(2,0); + stAxes[0][0] = -Rot.at< float >(0, 0); + stAxes[0][1] = -Rot.at< float >(1, 0); + stAxes[0][2] = +Rot.at< float >(2, 0); // y axis - stAxes[1][0] = -Rot.at(0,1); - stAxes[1][1] = -Rot.at(1,1); - stAxes[1][2] = +Rot.at(2,1); + stAxes[1][0] = -Rot.at< float >(0, 1); + stAxes[1][1] = -Rot.at< float >(1, 1); + stAxes[1][2] = +Rot.at< float >(2, 1); // for z axis, we use cross product - stAxes[2][0] = stAxes[0][1]*stAxes[1][2] - stAxes[0][2]*stAxes[1][1]; - stAxes[2][1] = - stAxes[0][0]*stAxes[1][2] + stAxes[0][2]*stAxes[1][0]; - stAxes[2][2] = stAxes[0][0]*stAxes[1][1] - stAxes[0][1]*stAxes[1][0]; + stAxes[2][0] = stAxes[0][1] * stAxes[1][2] - stAxes[0][2] * stAxes[1][1]; + stAxes[2][1] = -stAxes[0][0] * stAxes[1][2] + stAxes[0][2] * stAxes[1][0]; + stAxes[2][2] = stAxes[0][0] * stAxes[1][1] - stAxes[0][1] * stAxes[1][0]; // transposed matrix double axes[3][3]; @@ -180,188 +178,174 @@ void Marker::OgreGetPoseParameters(double position[3], double orientation[4]) th // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes // article "Quaternion Calculus and Fast Animation". - double fTrace = axes[0][0]+axes[1][1]+axes[2][2]; + double fTrace = axes[0][0] + axes[1][1] + axes[2][2]; double fRoot; - if ( fTrace > 0.0 ) - { - // |w| > 1/2, may as well choose w > 1/2 - fRoot = sqrt(fTrace + 1.0); // 2w - orientation[0] = 0.5*fRoot; - fRoot = 0.5/fRoot; // 1/(4w) - orientation[1] = (axes[2][1]-axes[1][2])*fRoot; - orientation[2] = (axes[0][2]-axes[2][0])*fRoot; - orientation[3] = (axes[1][0]-axes[0][1])*fRoot; - } - else - { - // |w| <= 1/2 - static unsigned int s_iNext[3] = { 1, 2, 0 }; - unsigned int i = 0; - if ( axes[1][1] > axes[0][0] ) - i = 1; - if ( axes[2][2] > axes[i][i] ) - i = 2; - unsigned int j = s_iNext[i]; - unsigned int k = s_iNext[j]; - - fRoot = sqrt(axes[i][i]-axes[j][j]-axes[k][k] + 1.0); - double* apkQuat[3] = { &orientation[1], &orientation[2], &orientation[3] }; - *apkQuat[i] = 0.5*fRoot; - fRoot = 0.5/fRoot; - orientation[0] = (axes[k][j]-axes[j][k])*fRoot; - *apkQuat[j] = (axes[j][i]+axes[i][j])*fRoot; - *apkQuat[k] = (axes[k][i]+axes[i][k])*fRoot; + if (fTrace > 0.0) { + // |w| > 1/2, may as well choose w > 1/2 + fRoot = sqrt(fTrace + 1.0); // 2w + orientation[0] = 0.5 * fRoot; + fRoot = 0.5 / fRoot; // 1/(4w) + orientation[1] = (axes[2][1] - axes[1][2]) * fRoot; + orientation[2] = (axes[0][2] - axes[2][0]) * fRoot; + orientation[3] = (axes[1][0] - axes[0][1]) * fRoot; + } else { + // |w| <= 1/2 + static unsigned int s_iNext[3] = {1, 2, 0}; + unsigned int i = 0; + if (axes[1][1] > axes[0][0]) + i = 1; + if (axes[2][2] > axes[i][i]) + i = 2; + unsigned int j = s_iNext[i]; + unsigned int k = s_iNext[j]; + + fRoot = sqrt(axes[i][i] - axes[j][j] - axes[k][k] + 1.0); + double *apkQuat[3] = {&orientation[1], &orientation[2], &orientation[3]}; + *apkQuat[i] = 0.5 * fRoot; + fRoot = 0.5 / fRoot; + orientation[0] = (axes[k][j] - axes[j][k]) * fRoot; + *apkQuat[j] = (axes[j][i] + axes[i][j]) * fRoot; + *apkQuat[k] = (axes[k][i] + axes[i][k]) * fRoot; } - - } -void Marker::draw(Mat &in, Scalar color, int lineWidth ,bool writeId)const -{ - if (size()!=4) return; - cv::line( in,(*this)[0],(*this)[1],color,lineWidth,CV_AA); - cv::line( in,(*this)[1],(*this)[2],color,lineWidth,CV_AA); - cv::line( in,(*this)[2],(*this)[3],color,lineWidth,CV_AA); - cv::line( in,(*this)[3],(*this)[0],color,lineWidth,CV_AA); - cv::rectangle( in,(*this)[0]-Point2f(2,2),(*this)[0]+Point2f(2,2),Scalar(0,0,255,255),lineWidth,CV_AA); - cv::rectangle( in,(*this)[1]-Point2f(2,2),(*this)[1]+Point2f(2,2),Scalar(0,255,0,255),lineWidth,CV_AA); - cv::rectangle( in,(*this)[2]-Point2f(2,2),(*this)[2]+Point2f(2,2),Scalar(255,0,0,255),lineWidth,CV_AA); +void Marker::draw(Mat &in, Scalar color, int lineWidth, bool writeId) const { + if (size() != 4) + return; + cv::line(in, (*this)[0], (*this)[1], color, lineWidth, CV_AA); + cv::line(in, (*this)[1], (*this)[2], color, lineWidth, CV_AA); + cv::line(in, (*this)[2], (*this)[3], color, lineWidth, CV_AA); + cv::line(in, (*this)[3], (*this)[0], color, lineWidth, CV_AA); + cv::rectangle(in, (*this)[0] - Point2f(2, 2), (*this)[0] + Point2f(2, 2), Scalar(0, 0, 255, 255), lineWidth, CV_AA); + cv::rectangle(in, (*this)[1] - Point2f(2, 2), (*this)[1] + Point2f(2, 2), Scalar(0, 255, 0, 255), lineWidth, CV_AA); + cv::rectangle(in, (*this)[2] - Point2f(2, 2), (*this)[2] + Point2f(2, 2), Scalar(255, 0, 0, 255), lineWidth, CV_AA); if (writeId) { char cad[100]; - sprintf(cad,"id=%d",id); - //determine the centroid - Point cent(0,0); - for (int i=0;i<4;i++) - { - cent.x+=(*this)[i].x; - cent.y+=(*this)[i].y; + sprintf(cad, "id=%d", id); + // determine the centroid + Point cent(0, 0); + for (int i = 0; i < 4; i++) { + cent.x += (*this)[i].x; + cent.y += (*this)[i].y; } - cent.x/=4.; - cent.y/=4.; - putText(in,cad, cent,FONT_HERSHEY_SIMPLEX, 0.5, Scalar(255-color[0],255-color[1],255-color[2],255),2); + cent.x /= 4.; + cent.y /= 4.; + putText(in, cad, cent, FONT_HERSHEY_SIMPLEX, 0.5, Scalar(255 - color[0], 255 - color[1], 255 - color[2], 255), 2); } } /** */ -void Marker::calculateExtrinsics(float markerSize,const CameraParameters &CP,bool setYPerpendicular)throw(cv::Exception) -{ - if (!CP.isValid()) throw cv::Exception(9004,"!CP.isValid(): invalid camera parameters. It is not possible to calculate extrinsics","calculateExtrinsics",__FILE__,__LINE__); - calculateExtrinsics( markerSize,CP.CameraMatrix,CP.Distorsion,setYPerpendicular); +void Marker::calculateExtrinsics(float markerSize, const CameraParameters &CP, bool setYPerpendicular) throw(cv::Exception) { + if (!CP.isValid()) + throw cv::Exception(9004, "!CP.isValid(): invalid camera parameters. It is not possible to calculate extrinsics", "calculateExtrinsics", __FILE__, + __LINE__); + calculateExtrinsics(markerSize, CP.CameraMatrix, CP.Distorsion, setYPerpendicular); } -void print(cv::Point3f p,string cad){ - cout<(1,0)=-halfSize; - ObjPoints.at(1,1)=halfSize; - ObjPoints.at(1,2)=0; - ObjPoints.at(2,0)=halfSize; - ObjPoints.at(2,1)=halfSize; - ObjPoints.at(2,2)=0; - ObjPoints.at(3,0)=halfSize; - ObjPoints.at(3,1)=-halfSize; - ObjPoints.at(3,2)=0; - ObjPoints.at(0,0)=-halfSize; - ObjPoints.at(0,1)=-halfSize; - ObjPoints.at(0,2)=0; - - cv::Mat ImagePoints(4,2,CV_32FC1); - - //Set image points from the marker - for (int c=0;c<4;c++) - { - ImagePoints.at(c,0)=((*this)[c].x); - ImagePoints.at(c,1)=((*this)[c].y); +void Marker::calculateExtrinsics(float markerSizeMeters, cv::Mat camMatrix, cv::Mat distCoeff, bool setYPerpendicular) throw(cv::Exception) { + if (!isValid()) + throw cv::Exception(9004, "!isValid(): invalid marker. It is not possible to calculate extrinsics", "calculateExtrinsics", __FILE__, __LINE__); + if (markerSizeMeters <= 0) + throw cv::Exception(9004, "markerSize<=0: invalid markerSize", "calculateExtrinsics", __FILE__, __LINE__); + if (camMatrix.rows == 0 || camMatrix.cols == 0) + throw cv::Exception(9004, "CameraMatrix is empty", "calculateExtrinsics", __FILE__, __LINE__); + + double halfSize = markerSizeMeters / 2.; + cv::Mat ObjPoints(4, 3, CV_32FC1); + ObjPoints.at< float >(1, 0) = -halfSize; + ObjPoints.at< float >(1, 1) = halfSize; + ObjPoints.at< float >(1, 2) = 0; + ObjPoints.at< float >(2, 0) = halfSize; + ObjPoints.at< float >(2, 1) = halfSize; + ObjPoints.at< float >(2, 2) = 0; + ObjPoints.at< float >(3, 0) = halfSize; + ObjPoints.at< float >(3, 1) = -halfSize; + ObjPoints.at< float >(3, 2) = 0; + ObjPoints.at< float >(0, 0) = -halfSize; + ObjPoints.at< float >(0, 1) = -halfSize; + ObjPoints.at< float >(0, 2) = 0; + + cv::Mat ImagePoints(4, 2, CV_32FC1); + + // Set image points from the marker + for (int c = 0; c < 4; c++) { + ImagePoints.at< float >(c, 0) = ((*this)[c].x); + ImagePoints.at< float >(c, 1) = ((*this)[c].y); } - cv::Mat raux,taux; - cv::solvePnP(ObjPoints, ImagePoints, camMatrix, distCoeff,raux,taux); - raux.convertTo(Rvec,CV_32F); - taux.convertTo(Tvec ,CV_32F); - //rotate the X axis so that Y is perpendicular to the marker plane - if (setYPerpendicular) rotateXAxis(Rvec); - ssize=markerSizeMeters; - //cout<<(*this)<(1,1)=cos(angleRad); - RX.at(1,2)=-sin(angleRad); - RX.at(2,1)=sin(angleRad); - RX.at(2,2)=cos(angleRad); - //now multiply - R=R*RX; - //finally, the the rodrigues back - Rodrigues(R,rotation); + // create a rotation matrix for x axis + cv::Mat RX = cv::Mat::eye(3, 3, CV_32F); + float angleRad = M_PI / 2; + RX.at< float >(1, 1) = cos(angleRad); + RX.at< float >(1, 2) = -sin(angleRad); + RX.at< float >(2, 1) = sin(angleRad); + RX.at< float >(2, 2) = cos(angleRad); + // now multiply + R = R * RX; + // finally, the the rodrigues back + Rodrigues(R, rotation); } /** */ -cv::Point2f Marker::getCenter()const -{ - cv::Point2f cent(0,0); - for(size_t i=0;i -{ -public: - //id of the marker +class ARUCO_EXPORTS Marker : public std::vector< cv::Point2f > { + public: + // id of the marker int id; - //size of the markers sides in meters + // size of the markers sides in meters float ssize; - //matrices of rotation and translation respect to the camera - cv::Mat Rvec,Tvec; + // matrices of rotation and translation respect to the camera + cv::Mat Rvec, Tvec; /** */ @@ -56,37 +55,37 @@ class ARUCO_EXPORTS Marker: public std::vector Marker(const Marker &M); /** */ - Marker(const std::vector &corners,int _id=-1); + Marker(const std::vector< cv::Point2f > &corners, int _id = -1); /** */ ~Marker() {} /**Indicates if this object is valid */ - bool isValid()const{return id!=-1 && size()==4;} + bool isValid() const { return id != -1 && size() == 4; } /**Draws this marker in the input image */ - void draw(cv::Mat &in, cv::Scalar color, int lineWidth=1,bool writeId=true)const; + void draw(cv::Mat &in, cv::Scalar color, int lineWidth = 1, bool writeId = true) const; /**Calculates the extrinsics (Rvec and Tvec) of the marker with respect to the camera * @param markerSize size of the marker side expressed in meters * @param CP parmeters of the camera * @param setYPerpendicular If set the Y axis will be perpendicular to the surface. Otherwise, it will be the Z axis */ - void calculateExtrinsics(float markerSize,const CameraParameters &CP,bool setYPerpendicular=true)throw(cv::Exception); + void calculateExtrinsics(float markerSize, const CameraParameters &CP, bool setYPerpendicular = true) throw(cv::Exception); /**Calculates the extrinsics (Rvec and Tvec) of the marker with respect to the camera * @param markerSize size of the marker side expressed in meters * @param CameraMatrix matrix with camera parameters (fx,fy,cx,cy) * @param Distorsion matrix with distorsion parameters (k1,k2,p1,p2) * @param setYPerpendicular If set the Y axis will be perpendicular to the surface. Otherwise, it will be the Z axis */ - void calculateExtrinsics(float markerSize,cv::Mat CameraMatrix,cv::Mat Distorsion=cv::Mat(),bool setYPerpendicular=true)throw(cv::Exception); - + void calculateExtrinsics(float markerSize, cv::Mat CameraMatrix, cv::Mat Distorsion = cv::Mat(), bool setYPerpendicular = true) throw(cv::Exception); + /**Given the extrinsic camera parameters returns the GL_MODELVIEW matrix for opengl. * Setting this matrix, the reference coordinate system will be set in this marker */ - void glGetModelViewMatrix( double modelview_matrix[16])throw(cv::Exception); - + void glGetModelViewMatrix(double modelview_matrix[16]) throw(cv::Exception); + /** * Returns position vector and orientation quaternion for an Ogre scene node or entity. * Use: @@ -97,47 +96,41 @@ class ARUCO_EXPORTS Marker: public std::vector * mySceneNode->setOrientation( ogreOrient ); * ... */ - void OgreGetPoseParameters( double position[3], double orientation[4] )throw(cv::Exception); - - /**Returns the centroid of the marker - */ - cv::Point2f getCenter()const; - /**Returns the perimeter of the marker - */ - float getPerimeter()const; + void OgreGetPoseParameters(double position[3], double orientation[4]) throw(cv::Exception); + + /**Returns the centroid of the marker + */ + cv::Point2f getCenter() const; + /**Returns the perimeter of the marker + */ + float getPerimeter() const; /**Returns the area */ - float getArea()const; + float getArea() const; /** */ /** */ - friend bool operator<(const Marker &M1,const Marker&M2) - { - return M1.id -#include +#include +#include +#include +#include +#include #include #include #include "arucofidmarkers.h" @@ -36,29 +39,28 @@ or implied, of Rafael Muñoz Salinas. #include "ar_omp.h" using namespace std; using namespace cv; - -namespace aruco -{ + +namespace aruco { /************************************ * * * * ************************************/ -MarkerDetector::MarkerDetector() -{ - _doErosion=false; - _thresMethod=ADPT_THRES; - _thresParam1=_thresParam2=7; - _cornerMethod=LINES; - _markerWarpSize=56; - _speed=0; - markerIdDetector_ptrfunc=aruco::FiducidalMarkers::detect; - pyrdown_level=0; // no image reduction - _minSize=0.04; - _maxSize=0.5; - - _borderDistThres=0.01;//corners in a border of 1% of image are ignored +MarkerDetector::MarkerDetector() { + _thresMethod = ADPT_THRES; + _thresParam1 = _thresParam2 = 7; + _cornerMethod = LINES; + _useLockedCorners = false; + // _cornerMethod=SUBPIX; + _thresParam1_range = 0; + _markerWarpSize = 56; + _speed = 0; + markerIdDetector_ptrfunc = aruco::FiducidalMarkers::detect; + _minSize = 0.04; + _maxSize = 0.5; + + _borderDistThres = 0.025; // corners in a border of 2.5% of image are ignored } /************************************ * @@ -67,10 +69,7 @@ MarkerDetector::MarkerDetector() * ************************************/ -MarkerDetector::~MarkerDetector() -{ - -} +MarkerDetector::~MarkerDetector() {} /************************************ * @@ -78,25 +77,24 @@ MarkerDetector::~MarkerDetector() * * ************************************/ -void MarkerDetector::setDesiredSpeed ( int val ) -{ - if ( val<0 ) val=0; - else if ( val>3 ) val=2; +void MarkerDetector::setDesiredSpeed(int val) { + if (val < 0) + val = 0; + else if (val > 3) + val = 2; - _speed=val; - switch ( _speed ) - { + _speed = val; + switch (_speed) { case 0: - _markerWarpSize=56; - _cornerMethod=SUBPIX; - _doErosion=true; + _markerWarpSize = 56; + _cornerMethod = SUBPIX; break; case 1: case 2: - _markerWarpSize=28; - _cornerMethod=NONE; + _markerWarpSize = 28; + _cornerMethod = NONE; break; }; } @@ -107,167 +105,165 @@ void MarkerDetector::setDesiredSpeed ( int val ) * * ************************************/ -void MarkerDetector::detect ( const cv::Mat &input,std::vector &detectedMarkers, CameraParameters camParams ,float markerSizeMeters ,bool setYPerpendicular) throw ( cv::Exception ) -{ - detect ( input, detectedMarkers,camParams.CameraMatrix ,camParams.Distorsion, markerSizeMeters ,setYPerpendicular); +void MarkerDetector::detect(const cv::Mat &input, std::vector< Marker > &detectedMarkers, CameraParameters camParams, float markerSizeMeters, + bool setYPerpendicular) throw(cv::Exception) { + detect(input, detectedMarkers, camParams.CameraMatrix, camParams.Distorsion, markerSizeMeters, setYPerpendicular); } - +/*** + * + * + **/ +void MarkerDetector::enableLockedCornersMethod(bool enable) { + _useLockedCorners = enable; + if (enable) + _cornerMethod = SUBPIX; +} /************************************ * * Main detection function. Performs all steps * * ************************************/ -void MarkerDetector::detect ( const cv::Mat &input,vector &detectedMarkers,Mat camMatrix ,Mat distCoeff ,float markerSizeMeters ,bool setYPerpendicular) throw ( cv::Exception ) -{ +void MarkerDetector::detect(const cv::Mat &input, vector< Marker > &detectedMarkers, Mat camMatrix, Mat distCoeff, float markerSizeMeters, + bool setYPerpendicular) throw(cv::Exception) { + + // it must be a 3 channel image + if (input.type() == CV_8UC3) + cv::cvtColor(input, grey, CV_BGR2GRAY); + else + grey = input; + double t1 = cv::getTickCount(); + // cv::cvtColor(grey,_ssImC ,CV_GRAY2BGR); //DELETE + + // clear input data + detectedMarkers.clear(); + + + cv::Mat imgToBeThresHolded = grey; + double ThresParam1 = _thresParam1, ThresParam2 = _thresParam2; + /// Do threshold the image and detect contours + // work simultaneouly in a range of values of the first threshold + int n_param1 = 2 * _thresParam1_range + 1; + vector< cv::Mat > thres_images(n_param1); +#pragma omp parallel for + for (int i = 0; i < n_param1; i++) { + double t1 = ThresParam1 - _thresParam1_range + _thresParam1_range * i; + thresHold(_thresMethod, imgToBeThresHolded, thres_images[i], t1, ThresParam2); + } + thres = thres_images[n_param1 / 2]; + // + + double t2 = cv::getTickCount(); + // find all rectangles in the thresholdes image + vector< MarkerCandidate > MarkerCanditates; + detectRectangles(thres_images, MarkerCanditates); + + double t3 = cv::getTickCount(); + + /// identify the markers + vector< vector< Marker > > markers_omp(omp_get_max_threads()); + vector< vector< std::vector< cv::Point2f > > > candidates_omp(omp_get_max_threads()); +//#pragma omp parallel for + for (int i = 0; i < MarkerCanditates.size(); i++) { + // Find proyective homography + Mat canonicalMarker; + bool resW = false; + resW = warp(grey, canonicalMarker, Size(_markerWarpSize, _markerWarpSize), MarkerCanditates[i]); + if (resW) { + int nRotations; + int id = (*markerIdDetector_ptrfunc)(canonicalMarker, nRotations); + if (id != -1) { + if (_cornerMethod == LINES) // make LINES refinement before lose contour points + refineCandidateLines(MarkerCanditates[i], camMatrix, distCoeff); + markers_omp[omp_get_thread_num()].push_back(MarkerCanditates[i]); + markers_omp[omp_get_thread_num()].back().id = id; + // sort the points so that they are always in the same order no matter the camera orientation + std::rotate(markers_omp[omp_get_thread_num()].back().begin(), markers_omp[omp_get_thread_num()].back().begin() + 4 - nRotations, + markers_omp[omp_get_thread_num()].back().end()); + } else + candidates_omp[omp_get_thread_num()].push_back(MarkerCanditates[i]); + } + } + // unify parallel data + joinVectors(markers_omp, detectedMarkers, true); + joinVectors(candidates_omp, _candidates, true); - //it must be a 3 channel image - if ( input.type() ==CV_8UC3 ) cv::cvtColor ( input,grey,CV_BGR2GRAY ); - else grey=input; + double t4 = cv::getTickCount(); -// cv::cvtColor(grey,_ssImC ,CV_GRAY2BGR); //DELETE + /// refine the corner location if desired + if (detectedMarkers.size() > 0 && _cornerMethod != NONE && _cornerMethod != LINES) { - //clear input data - detectedMarkers.clear(); + vector< Point2f > Corners; + for (unsigned int i = 0; i < detectedMarkers.size(); i++) + for (int c = 0; c < 4; c++) + Corners.push_back(detectedMarkers[i][c]); + // in case of "locked corners", it is neccesary in some ocasions to + // find the corner in the sourrondings of the initially estimated location + if (_useLockedCorners) + findCornerMaxima(Corners, grey, _thresParam1); - cv::Mat imgToBeThresHolded=grey; - double ThresParam1=_thresParam1,ThresParam2=_thresParam2; - //Must the image be downsampled before continue pocessing? - if ( pyrdown_level!=0 ) - { - reduced=grey; - for ( int i=0;i MarkerCanditates; - detectRectangles ( thres,MarkerCanditates ); - //if the image has been downsampled, then calcualte the location of the corners in the original image - if ( pyrdown_level!=0 ) - { - float red_den=pow ( 2.0f,pyrdown_level ); - float offInc= ( ( pyrdown_level/2. )-0.5 ); - for ( unsigned int i=0;i toRemove(detectedMarkers.size(), false); + for (int i = 0; i < int(detectedMarkers.size()) - 1; i++) { + if (detectedMarkers[i].id == detectedMarkers[i + 1].id && !toRemove[i + 1]) { + // deletes the one with smaller perimeter + if (perimeter(detectedMarkers[i]) > perimeter(detectedMarkers[i + 1])) + toRemove[i + 1] = true; + else + toRemove[i] = true; } } - - ///identify the markers - vector >markers_omp(omp_get_max_threads()); - vector > >candidates_omp(omp_get_max_threads()); - #pragma omp parallel for - for ( unsigned int i=0;i input.cols - borderDistThresX || detectedMarkers[i][c].y > input.rows - borderDistThresY) { + toRemove[i] = true; } - else candidates_omp[omp_get_thread_num()].push_back ( MarkerCanditates[i] ); } - } - //unify parallel data - joinVectors(markers_omp,detectedMarkers,true); - joinVectors(candidates_omp,_candidates,true); - - ///refine the corner location if desired - if ( detectedMarkers.size() >0 && _cornerMethod!=NONE && _cornerMethod!=LINES ) - { - vector Corners; - for ( unsigned int i=0;i toRemove ( detectedMarkers.size(),false ); - for ( int i=0;iperimeter ( detectedMarkers[i+1] ) ) toRemove[i+1]=true; - else toRemove[i]=true; - } - //delete if any of the corners is too near image border - for(size_t c=0;cinput.cols-borderDistThresX || - detectedMarkers[i][c].y>input.rows-borderDistThresY ) toRemove[i]=true; - - } - - - } - //remove the markers marker - removeElements ( detectedMarkers, toRemove ); - - ///detect the position of detected markers if desired - if ( camMatrix.rows!=0 && markerSizeMeters>0 ) - { - for ( unsigned int i=0;i 0) { + for (unsigned int i = 0; i < detectedMarkers.size(); i++) + detectedMarkers[i].calculateExtrinsics(markerSizeMeters, camMatrix, distCoeff, setYPerpendicular); } + double t6 = cv::getTickCount(); + +// cerr << "Threshold: " << (t2 - t1) / double(cv::getTickFrequency()) << endl; +// cerr << "Rectangles: " << (t3 - t2) / double(cv::getTickFrequency()) << endl; +// cerr << "Identify: " << (t4 - t3) / double(cv::getTickFrequency()) << endl; +// cerr << "Subpixel: " << (t5 - t4) / double(cv::getTickFrequency()) << endl; +// cerr << "Filtering: " << (t6 - t5) / double(cv::getTickFrequency()) << endl; } @@ -277,151 +273,159 @@ void MarkerDetector::detect ( const cv::Mat &input,vector &detectedMark * * ************************************/ -void MarkerDetector::detectRectangles ( const cv::Mat &thres,vector > &MarkerCanditates ) -{ - vector candidates; - detectRectangles(thres,candidates); - //create the output +void MarkerDetector::detectRectangles(const cv::Mat &thres, vector< std::vector< cv::Point2f > > &MarkerCanditates) { + vector< MarkerCandidate > candidates; + vector< cv::Mat > thres_v; + thres_v.push_back(thres); + detectRectangles(thres_v, candidates); + // create the output MarkerCanditates.resize(candidates.size()); - for (size_t i=0;i & OutMarkerCanditates) -{ - vector MarkerCanditates; - //calcualte the min_max contour sizes - int minSize=_minSize*std::max(thresImg.cols,thresImg.rows)*4; - int maxSize=_maxSize*std::max(thresImg.cols,thresImg.rows)*4; - std::vector > contours2; - std::vector hierarchy2; - - thresImg.copyTo ( thres2 ); - cv::findContours ( thres2 , contours2, hierarchy2,CV_RETR_LIST, CV_CHAIN_APPROX_NONE ); - vector approxCurve; - ///for each contour, analyze if it is a paralelepiped likely to be the marker - - for ( unsigned int i=0;i10 ) - { - //add the points - // cout<<"ADDED"< &thresImgv, vector< MarkerCandidate > &OutMarkerCanditates) { + // omp_set_num_threads ( 1 ); + vector< vector< MarkerCandidate > > MarkerCanditatesV(omp_get_max_threads()); + // calcualte the min_max contour sizes + int minSize = _minSize * std::max(thresImgv[0].cols, thresImgv[0].rows) * 4; + int maxSize = _maxSize * std::max(thresImgv[0].cols, thresImgv[0].rows) * 4; + +// cv::Mat input; +// cv::cvtColor ( thresImgv[0],input,CV_GRAY2BGR ); + +#pragma omp parallel for + for (size_t i = 0; i < thresImgv.size(); i++) { + std::vector< cv::Vec4i > hierarchy2; + std::vector< std::vector< cv::Point > > contours2; + cv::Mat thres2; + thresImgv[i].copyTo(thres2); + cv::findContours(thres2, contours2, hierarchy2, CV_RETR_LIST, CV_CHAIN_APPROX_NONE); + + + vector< Point > approxCurve; + /// for each contour, analyze if it is a paralelepiped likely to be the marker + for (unsigned int i = 0; i < contours2.size(); i++) { + + // check it is a possible element by first checking is has enough points + if (minSize < contours2[i].size() && contours2[i].size() < maxSize) { + // approximate to a poligon + approxPolyDP(contours2[i], approxCurve, double(contours2[i].size()) * 0.05, true); + // drawApproxCurve(copy,approxCurve,Scalar(0,0,255)); + // check that the poligon has 4 points + if (approxCurve.size() == 4) { + /* + drawContour ( input,contours2[i],Scalar ( 255,0,225 ) ); + namedWindow ( "input" ); + imshow ( "input",input );*/ + // waitKey(0); + // and is convex + if (isContourConvex(Mat(approxCurve))) { + // drawApproxCurve(input,approxCurve,Scalar(255,0,255)); + // //ensure that the distace between consecutive points is large enough + float minDist = 1e10; + for (int j = 0; j < 4; j++) { + float d = std::sqrt((float)(approxCurve[j].x - approxCurve[(j + 1) % 4].x) * (approxCurve[j].x - approxCurve[(j + 1) % 4].x) + + (approxCurve[j].y - approxCurve[(j + 1) % 4].y) * (approxCurve[j].y - approxCurve[(j + 1) % 4].y)); + // norm(Mat(approxCurve[i]),Mat(approxCurve[(i+1)%4])); + if (d < minDist) + minDist = d; + } + // check that distance is not very small + if (minDist > 10) { + // add the points + // cout<<"ADDED"< MarkerCanditates; -// namedWindow("input"); -// imshow("input",input); -// waitKey(0); - ///sort the points in anti-clockwise order - valarray swapped(false,MarkerCanditates.size());//used later - for ( unsigned int i=0;i swapped(false, MarkerCanditates.size()); // used later + for (unsigned int i = 0; i < MarkerCanditates.size(); i++) { - //trace a line between the first and second point. - //if the thrid point is at the right side, then the points are anti-clockwise + // trace a line between the first and second point. + // if the thrid point is at the right side, then the points are anti-clockwise double dx1 = MarkerCanditates[i][1].x - MarkerCanditates[i][0].x; - double dy1 = MarkerCanditates[i][1].y - MarkerCanditates[i][0].y; + double dy1 = MarkerCanditates[i][1].y - MarkerCanditates[i][0].y; double dx2 = MarkerCanditates[i][2].x - MarkerCanditates[i][0].x; double dy2 = MarkerCanditates[i][2].y - MarkerCanditates[i][0].y; - double o = ( dx1*dy2 )- ( dy1*dx2 ); - - if ( o < 0.0 ) //if the third point is in the left side, then sort in anti-clockwise order - { - swap ( MarkerCanditates[i][1],MarkerCanditates[i][3] ); - swapped[i]=true; - //sort the contour points -// reverse(MarkerCanditates[i].contour.begin(),MarkerCanditates[i].contour.end());//???? + double o = (dx1 * dy2) - (dy1 * dx2); + if (o < 0.0) { // if the third point is in the left side, then sort in anti-clockwise order + swap(MarkerCanditates[i][1], MarkerCanditates[i][3]); + swapped[i] = true; + // sort the contour points + // reverse(MarkerCanditates[i].contour.begin(),MarkerCanditates[i].contour.end());//???? } } - + /// remove these elements which corners are too close to each other - //first detect candidates to be removed - - vector< vector > > TooNearCandidates_omp(omp_get_max_threads()); - #pragma omp parallel for - for ( unsigned int i=0;i > > TooNearCandidates_omp(omp_get_max_threads()); +#pragma omp parallel for + for (unsigned int i = 0; i < MarkerCanditates.size(); i++) { // cout<<"Marker i="< ( i,j ) ); + // calculate the average distance of each corner to the nearest corner of the other marker candidate + for (unsigned int j = i + 1; j < MarkerCanditates.size(); j++) { + valarray< float > vdist(4); + for (int c = 0; c < 4; c++) + vdist[c] = sqrt((MarkerCanditates[i][c].x - MarkerCanditates[j][c].x) * (MarkerCanditates[i][c].x - MarkerCanditates[j][c].x) + + (MarkerCanditates[i][c].y - MarkerCanditates[j][c].y) * (MarkerCanditates[i][c].y - MarkerCanditates[j][c].y)); + // dist/=4; + // if distance is too small + if (vdist[0] < 6 && vdist[1] < 6 && vdist[2] < 6 && vdist[3] < 6) { + TooNearCandidates_omp[omp_get_thread_num()].push_back(pair< int, int >(i, j)); } } } - //join - vector > TooNearCandidates; - joinVectors( TooNearCandidates_omp,TooNearCandidates); - //mark for removal the element of the pair with smaller perimeter - valarray toRemove ( false,MarkerCanditates.size() ); - for ( unsigned int i=0;iperimeter ( MarkerCanditates[ TooNearCandidates[i].second] ) ) - toRemove[TooNearCandidates[i].second]=true; - else toRemove[TooNearCandidates[i].first]=true; + // join + vector< pair< int, int > > TooNearCandidates; + joinVectors(TooNearCandidates_omp, TooNearCandidates); + // mark for removal the element of the pair with smaller perimeter + valarray< bool > toRemove(false, MarkerCanditates.size()); + for (unsigned int i = 0; i < TooNearCandidates.size(); i++) { + if (perimeter(MarkerCanditates[TooNearCandidates[i].first]) > perimeter(MarkerCanditates[TooNearCandidates[i].second])) + toRemove[TooNearCandidates[i].second] = true; + else + toRemove[TooNearCandidates[i].first] = true; } - //remove the invalid ones -// removeElements ( MarkerCanditates,toRemove ); - //finally, assign to the remaining candidates the contour + // remove the invalid ones + // finally, assign to the remaining candidates the contour OutMarkerCanditates.reserve(MarkerCanditates.size()); - for (size_t i=0;i points ) throw ( cv::Exception ) -{ - - if ( points.size() !=4 ) throw cv::Exception ( 9001,"point.size()!=4","MarkerDetector::warp",__FILE__,__LINE__ ); - //obtain the perspective transform - Point2f pointsRes[4],pointsIn[4]; - for ( int i=0;i<4;i++ ) pointsIn[i]=points[i]; - pointsRes[0]= ( Point2f ( 0,0 ) ); - pointsRes[1]= Point2f ( size.width-1,0 ); - pointsRes[2]= Point2f ( size.width-1,size.height-1 ); - pointsRes[3]= Point2f ( 0,size.height-1 ); - Mat M=getPerspectiveTransform ( pointsIn,pointsRes ); - cv::warpPerspective ( in, out, M, size,cv::INTER_NEAREST ); +bool MarkerDetector::warp(Mat &in, Mat &out, Size size, vector< Point2f > points) throw(cv::Exception) { + + if (points.size() != 4) + throw cv::Exception(9001, "point.size()!=4", "MarkerDetector::warp", __FILE__, __LINE__); + // obtain the perspective transform + Point2f pointsRes[4], pointsIn[4]; + for (int i = 0; i < 4; i++) + pointsIn[i] = points[i]; + pointsRes[0] = (Point2f(0, 0)); + pointsRes[1] = Point2f(size.width - 1, 0); + pointsRes[2] = Point2f(size.width - 1, size.height - 1); + pointsRes[3] = Point2f(0, size.height - 1); + Mat M = getPerspectiveTransform(pointsIn, pointsRes); + cv::warpPerspective(in, out, M, size, cv::INTER_NEAREST); return true; } -void findCornerPointsInContour(const vector& points,const vector &contour,vector &idxs) -{ - assert(points.size()==4); - int idxSegments[4]={-1,-1,-1,-1}; - //the first point coincides with one +void findCornerPointsInContour(const vector< cv::Point2f > &points, const vector< cv::Point > &contour, vector< int > &idxs) { + assert(points.size() == 4); + int idxSegments[4] = {-1, -1, -1, -1}; + // the first point coincides with one cv::Point points2i[4]; - for (int i=0;i<4;i++) { - points2i[i].x=points[i].x; - points2i[i].y=points[i].y; + for (int i = 0; i < 4; i++) { + points2i[i].x = points[i].x; + points2i[i].y = points[i].y; } - for (size_t i=0;i &contour,const vector &idxSegments) -{ - float distSum[4]={0,0,0,0}; - cv::Scalar colors[4]={cv::Scalar(0,0,255),cv::Scalar(255,0,0),cv::Scalar(0,255,0),cv::Scalar(111,111,0)}; +int findDeformedSidesIdx(const vector< cv::Point > &contour, const vector< int > &idxSegments) { + float distSum[4] = {0, 0, 0, 0}; + cv::Scalar colors[4] = {cv::Scalar(0, 0, 255), cv::Scalar(255, 0, 0), cv::Scalar(0, 255, 0), cv::Scalar(111, 111, 0)}; - for (int i=0;i<3;i++) { - cv::Point p1=contour[ idxSegments[i]]; - cv::Point p2=contour[ idxSegments[i+1]]; - float inv_den=1./ sqrt(float(( p2.x-p1.x)*(p2.x-p1.x)+ (p2.y-p1.y)*(p2.y-p1.y))); + for (int i = 0; i < 3; i++) { + cv::Point p1 = contour[idxSegments[i]]; + cv::Point p2 = contour[idxSegments[i + 1]]; + float inv_den = 1. / sqrt(float((p2.x - p1.x) * (p2.x - p1.x) + (p2.y - p1.y) * (p2.y - p1.y))); // d=|v^^·r|=(|(x_2-x_1)(y_1-y_0)-(x_1-x_0)(y_2-y_1)|)/(sqrt((x_2-x_1)^2+(y_2-y_1)^2)). -// cerr<<"POSS="< pointsCO(mcand.contour.size()); - assert(M.type()==CV_64F); - assert(M.cols==3 && M.rows==3); -// cout<(0); + // cv::imshow("imC",_ssImC); + + + // calculate the max distance from each contour point the line of the corresponding segment it belongs to + // calculate + // cv::waitKey(0); + // check that the region is into image limits + // obtain the perspective transform + Point2f pointsRes[4], pointsIn[4]; + for (int i = 0; i < 4; i++) + pointsIn[i] = mcand[i]; + + cv::Size enlargedSize = size; + enlargedSize.width += 2 * enlargedSize.width * 0.2; + pointsRes[0] = (Point2f(0, 0)); + pointsRes[1] = Point2f(enlargedSize.width - 1, 0); + pointsRes[2] = Point2f(enlargedSize.width - 1, enlargedSize.height - 1); + pointsRes[3] = Point2f(0, enlargedSize.height - 1); + // rotate to ensure that deformed sides are in the horizontal axis when warping + if (defrmdSide == 0) + rotate(pointsRes, pointsRes + 1, pointsRes + 4); + cv::Mat imAux, imAux2(enlargedSize, CV_8UC1); + Mat M = cv::getPerspectiveTransform(enlargedRegion, pointsRes); + cv::warpPerspective(in, imAux, M, enlargedSize, cv::INTER_NEAREST); + + // now, transform all points to the new image + vector< cv::Point > pointsCO(mcand.contour.size()); + assert(M.type() == CV_64F); + assert(M.cols == 3 && M.rows == 3); + // cout<(0); imAux2.setTo(cv::Scalar::all(0)); - for (size_t i=0;i(pointsCO[i].y,pointsCO[i].x)=255; - if (pointsCO[i].y>0) - imAux2.at(pointsCO[i].y-1,pointsCO[i].x)=255; - if (pointsCO[i].y(pointsCO[i].y+1,pointsCO[i].x)=255; + pointsCO[i].x = ((inX * mptr[0] + inY * mptr[1] + mptr[2]) / w) + 0.5; + pointsCO[i].y = ((inX * mptr[3] + inY * mptr[4] + mptr[5]) / w) + 0.5; + // make integers + setPointIntoImage(pointsCO[i], imAux.size()); // ensure points are into image limits + // cout<<"p="<(pointsCO[i].y, pointsCO[i].x) = 255; + if (pointsCO[i].y > 0) + imAux2.at< uchar >(pointsCO[i].y - 1, pointsCO[i].x) = 255; + if (pointsCO[i].y < imAux2.rows - 1) + imAux2.at< uchar >(pointsCO[i].y + 1, pointsCO[i].x) = 255; } - cv::Mat outIm(enlargedSize,CV_8UC1); + cv::Mat outIm(enlargedSize, CV_8UC1); outIm.setTo(cv::Scalar::all(0)); - //now, scan in lines to determine the required displacement - for (int y=0;y(y); - int start=-1,end=-1; - //determine the start and end of markerd regions - for (int x=0;x(y); + int start = -1, end = -1; + // determine the start and end of markerd regions + for (int x = 0; x < imAux.cols; x++) { if (_offInfo[x]) { - if (start==-1) start=x; - else end=x; + if (start == -1) + start = x; + else + end = x; } } -// cout<<"S="<>1)< size.width>>1); - uchar *In_image=imAux.ptr(y); - uchar *Out_image=outIm.ptr(y); - memcpy(Out_image,In_image+start,imAux.cols-start ); + // cout<<"S="<>1)< size.width >> 1); + uchar *In_image = imAux.ptr< uchar >(y); + uchar *Out_image = outIm.ptr< uchar >(y); + memcpy(Out_image, In_image + start, imAux.cols - start); } -// cout<<"SS="< &b ) -{ +bool MarkerDetector::isInto(Mat &contour, vector< Point2f > &b) { - for ( unsigned int i=0;i0 ) return true; + for (unsigned int i = 0; i < b.size(); i++) + if (pointPolygonTest(contour, b[i], false) > 0) + return true; return false; } /************************************ @@ -731,13 +752,11 @@ bool MarkerDetector::isInto ( Mat &contour,vector &b ) * * ************************************/ -int MarkerDetector:: perimeter ( vector &a ) -{ - int sum=0; - for ( unsigned int i=0;i &a) { + int sum = 0; + for (unsigned int i = 0; i < a.size(); i++) { + int i2 = (i + 1) % a.size(); + sum += sqrt((a[i].x - a[i2].x) * (a[i].x - a[i2].x) + (a[i].y - a[i2].y) * (a[i].y - a[i2].y)); } return sum; } @@ -747,11 +766,9 @@ int MarkerDetector:: perimeter ( vector &a ) * * */ -void MarkerDetector::findBestCornerInRegion_harris ( const cv::Mat & grey,vector & Corners,int blockSize ) -{ - SubPixelCorner Subp; - Subp.RefineCorner(grey,Corners); - +void MarkerDetector::findBestCornerInRegion_harris(const cv::Mat &grey, vector< cv::Point2f > &Corners, int blockSize) { + SubPixelCorner Subp; + Subp.RefineCorner(grey, Corners); } @@ -759,168 +776,167 @@ void MarkerDetector::findBestCornerInRegion_harris ( const cv::Mat & grey,vecto * * */ -void MarkerDetector::refineCandidateLines(MarkerDetector::MarkerCandidate& candidate, const cv::Mat &camMatrix, const cv::Mat &distCoeff) -{ - // search corners on the contour vector - vector cornerIndex; - cornerIndex.resize(4); - for(unsigned int j=0; j cornerIndex[0]) && (cornerIndex[2]>cornerIndex[1] || cornerIndex[2]cornerIndex[1] && cornerIndex[2] contour2f; - for(unsigned int i=0; i > contourLines; - contourLines.resize(4); - for(unsigned int l=0; l<4; l++) { - for(int j=(int)cornerIndex[l]; j!=(int)cornerIndex[(l+1)%4]; j+=inc) { - if(j==(int)candidate.contour.size() && !inverse) j=0; - else if(j==0 && inverse) j=candidate.contour.size()-1; - contourLines[l].push_back(contour2f[j]); - if(j==(int)cornerIndex[(l+1)%4]) break; // this has to be added because of the previous ifs - } - - } - - // interpolate marker lines - vector lines; - lines.resize(4); - for(unsigned int j=0; j crossPoints; - crossPoints.resize(4); - for(unsigned int i=0; i<4; i++) - crossPoints[i] = getCrossPoint( lines[(i-1)%4], lines[i] ); - - // distort corners again if undistortion was performed - if(!camMatrix.empty() && !distCoeff.empty()) - distortPoints(crossPoints, crossPoints, camMatrix, distCoeff); - - // reassing points - for(unsigned int j=0; j<4; j++) - candidate[j] = crossPoints[j]; +void MarkerDetector::refineCandidateLines(MarkerDetector::MarkerCandidate &candidate, const cv::Mat &camMatrix, const cv::Mat &distCoeff) { + // search corners on the contour vector + vector< unsigned int > cornerIndex; + cornerIndex.resize(4); + for (unsigned int j = 0; j < candidate.contour.size(); j++) { + for (unsigned int k = 0; k < 4; k++) { + if (candidate.contour[j].x == candidate[k].x && candidate.contour[j].y == candidate[k].y) { + cornerIndex[k] = j; + } + } + } + + // contour pixel in inverse order or not? + bool inverse; + if ((cornerIndex[1] > cornerIndex[0]) && (cornerIndex[2] > cornerIndex[1] || cornerIndex[2] < cornerIndex[0])) + inverse = false; + else if (cornerIndex[2] > cornerIndex[1] && cornerIndex[2] < cornerIndex[0]) + inverse = false; + else + inverse = true; + + + // get pixel vector for each line of the marker + int inc = 1; + if (inverse) + inc = -1; + + // undistort contour + vector< Point2f > contour2f; + for (unsigned int i = 0; i < candidate.contour.size(); i++) + contour2f.push_back(cv::Point2f(candidate.contour[i].x, candidate.contour[i].y)); + if (!camMatrix.empty() && !distCoeff.empty()) + cv::undistortPoints(contour2f, contour2f, camMatrix, distCoeff, cv::Mat(), camMatrix); + + + vector< std::vector< cv::Point2f > > contourLines; + contourLines.resize(4); + for (unsigned int l = 0; l < 4; l++) { + for (int j = (int)cornerIndex[l]; j != (int)cornerIndex[(l + 1) % 4]; j += inc) { + if (j == (int)candidate.contour.size() && !inverse) + j = 0; + else if (j == 0 && inverse) + j = candidate.contour.size() - 1; + contourLines[l].push_back(contour2f[j]); + if (j == (int)cornerIndex[(l + 1) % 4]) + break; // this has to be added because of the previous ifs + } + } + + // interpolate marker lines + vector< Point3f > lines; + lines.resize(4); + for (unsigned int j = 0; j < lines.size(); j++) + interpolate2Dline(contourLines[j], lines[j]); + + // get cross points of lines + vector< Point2f > crossPoints; + crossPoints.resize(4); + for (unsigned int i = 0; i < 4; i++) + crossPoints[i] = getCrossPoint(lines[(i - 1) % 4], lines[i]); + + // distort corners again if undistortion was performed + if (!camMatrix.empty() && !distCoeff.empty()) + distortPoints(crossPoints, crossPoints, camMatrix, distCoeff); + + // reassing points + for (unsigned int j = 0; j < 4; j++) + candidate[j] = crossPoints[j]; } /** */ -void MarkerDetector::interpolate2Dline( const std::vector< Point2f >& inPoints, Point3f& outLine) -{ - - float minX, maxX, minY, maxY; - minX = maxX = inPoints[0].x; - minY = maxY = inPoints[0].y; - for(unsigned int i=1; i maxX) maxX = inPoints[i].x; - if(inPoints[i].y < minY) minY = inPoints[i].y; - if(inPoints[i].y > maxY) maxY = inPoints[i].y; - } +void MarkerDetector::interpolate2Dline(const std::vector< Point2f > &inPoints, Point3f &outLine) { + + float minX, maxX, minY, maxY; + minX = maxX = inPoints[0].x; + minY = maxY = inPoints[0].y; + for (unsigned int i = 1; i < inPoints.size(); i++) { + if (inPoints[i].x < minX) + minX = inPoints[i].x; + if (inPoints[i].x > maxX) + maxX = inPoints[i].x; + if (inPoints[i].y < minY) + minY = inPoints[i].y; + if (inPoints[i].y > maxY) + maxY = inPoints[i].y; + } // create matrices of equation system - Mat A(inPoints.size(),2,CV_32FC1, Scalar(0)); - Mat B(inPoints.size(),1,CV_32FC1, Scalar(0)); + Mat A(inPoints.size(), 2, CV_32FC1, Scalar(0)); + Mat B(inPoints.size(), 1, CV_32FC1, Scalar(0)); Mat X; - - - if( maxX-minX > maxY-minY ) { - // Ax + C = y - for (int i=0; i(i, 0) = inPoints[i].x; - A.at(i, 1) = 1.; - B.at(i, 0) = inPoints[i].y; - } - // solve system - solve(A,B,X, DECOMP_SVD); - // return Ax + By + C - outLine = Point3f(X.at(0,0), -1., X.at(1,0)); - } - else { - // By + C = x - for (int i=0; i maxY - minY) { + // Ax + C = y + for (int i = 0; i < inPoints.size(); i++) { - A.at(i, 0) = inPoints[i].y; - A.at(i, 1) = 1.; - B.at(i, 0) = inPoints[i].x; + A.at< float >(i, 0) = inPoints[i].x; + A.at< float >(i, 1) = 1.; + B.at< float >(i, 0) = inPoints[i].y; + } - } + // solve system + solve(A, B, X, DECOMP_SVD); + // return Ax + By + C + outLine = Point3f(X.at< float >(0, 0), -1., X.at< float >(1, 0)); + } else { + // By + C = x + for (int i = 0; i < inPoints.size(); i++) { + + A.at< float >(i, 0) = inPoints[i].y; + A.at< float >(i, 1) = 1.; + B.at< float >(i, 0) = inPoints[i].x; + } - // solve system - solve(A,B,X, DECOMP_SVD); - // return Ax + By + C - outLine = Point3f(-1., X.at(0,0), X.at(1,0)); + // solve system + solve(A, B, X, DECOMP_SVD); + // return Ax + By + C + outLine = Point3f(-1., X.at< float >(0, 0), X.at< float >(1, 0)); } - } /** */ -Point2f MarkerDetector::getCrossPoint(const cv::Point3f& line1, const cv::Point3f& line2) -{ - +Point2f MarkerDetector::getCrossPoint(const cv::Point3f &line1, const cv::Point3f &line2) { + // create matrices of equation system - Mat A(2,2,CV_32FC1, Scalar(0)); - Mat B(2,1,CV_32FC1, Scalar(0)); + Mat A(2, 2, CV_32FC1, Scalar(0)); + Mat B(2, 1, CV_32FC1, Scalar(0)); Mat X; - A.at(0, 0) = line1.x; - A.at(0, 1) = line1.y; - B.at(0, 0) = -line1.z; + A.at< float >(0, 0) = line1.x; + A.at< float >(0, 1) = line1.y; + B.at< float >(0, 0) = -line1.z; + + A.at< float >(1, 0) = line2.x; + A.at< float >(1, 1) = line2.y; + B.at< float >(1, 0) = -line2.z; - A.at(1, 0) = line2.x; - A.at(1, 1) = line2.y; - B.at(1, 0) = -line2.z; - // solve system - solve(A,B,X, DECOMP_SVD); - return Point2f(X.at(0,0), X.at(1,0)); - + solve(A, B, X, DECOMP_SVD); + return Point2f(X.at< float >(0, 0), X.at< float >(1, 0)); } /** */ -void MarkerDetector::distortPoints(vector in, vector &out, const Mat& camMatrix, const Mat& distCoeff) -{ - // trivial extrinsics - cv::Mat Rvec = cv::Mat(3,1,CV_32FC1, cv::Scalar::all(0)); - cv::Mat Tvec = Rvec.clone(); - // calculate 3d points and then reproject, so opencv makes the distortion internally - vector cornersPoints3d; - for(unsigned int i=0; i(0,2))/camMatrix.at(0,0), //x - (in[i].y-camMatrix.at(1,2))/camMatrix.at(1,1), //y - 1 ) ); //z - cv::projectPoints(cornersPoints3d, Rvec, Tvec, camMatrix, distCoeff, out); +void MarkerDetector::distortPoints(vector< cv::Point2f > in, vector< cv::Point2f > &out, const Mat &camMatrix, const Mat &distCoeff) { + // trivial extrinsics + cv::Mat Rvec = cv::Mat(3, 1, CV_32FC1, cv::Scalar::all(0)); + cv::Mat Tvec = Rvec.clone(); + // calculate 3d points and then reproject, so opencv makes the distortion internally + vector< cv::Point3f > cornersPoints3d; + for (unsigned int i = 0; i < in.size(); i++) + cornersPoints3d.push_back(cv::Point3f((in[i].x - camMatrix.at< float >(0, 2)) / camMatrix.at< float >(0, 0), // x + (in[i].y - camMatrix.at< float >(1, 2)) / camMatrix.at< float >(1, 1), // y + 1)); // z + cv::projectPoints(cornersPoints3d, Rvec, Tvec, camMatrix, distCoeff, out); } @@ -931,10 +947,7 @@ void MarkerDetector::distortPoints(vector in, vector & * * ************************************/ -void MarkerDetector::drawAllContours ( Mat input, std::vector > &contours ) -{ - drawContours ( input, contours, -1,Scalar ( 255,0,255 ) ); -} +void MarkerDetector::drawAllContours(Mat input, std::vector< std::vector< cv::Point > > &contours) { drawContours(input, contours, -1, Scalar(255, 0, 255)); } /************************************ * @@ -942,19 +955,15 @@ void MarkerDetector::drawAllContours ( Mat input, std::vector &contour,Scalar color ) -{ - for ( unsigned int i=0;i &contour, Scalar color) { + for (unsigned int i = 0; i < contour.size(); i++) { + cv::rectangle(in, contour[i], contour[i], color); } } -void MarkerDetector:: drawApproxCurve ( Mat &in,vector &contour,Scalar color ) -{ - for ( unsigned int i=0;i &contour, Scalar color) { + for (unsigned int i = 0; i < contour.size(); i++) { + cv::line(in, contour[i], contour[(i + 1) % contour.size()], color); } } /************************************ @@ -964,14 +973,12 @@ void MarkerDetector:: drawApproxCurve ( Mat &in,vector &contour,Scalar * ************************************/ -void MarkerDetector::draw ( Mat out,const vector &markers ) -{ - for ( unsigned int i=0;i &markers) { + for (unsigned int i = 0; i < markers.size(); i++) { + cv::line(out, markers[i][0], markers[i][1], cvScalar(255, 0, 0), 2, CV_AA); + cv::line(out, markers[i][1], markers[i][2], cvScalar(255, 0, 0), 2, CV_AA); + cv::line(out, markers[i][2], markers[i][3], cvScalar(255, 0, 0), 2, CV_AA); + cv::line(out, markers[i][3], markers[i][0], cvScalar(255, 0, 0), 2, CV_AA); } } /* Attempt to make it faster than in opencv. I could not :( Maybe trying with SSE3... @@ -1006,10 +1013,11 @@ void MarkerDetector::warpPerspective(const cv::Mat &in,cv::Mat & out, const cv:: * ************************************/ -void MarkerDetector::glGetProjectionMatrix ( CameraParameters & CamMatrix,cv::Size orgImgSize, cv::Size size,double proj_matrix[16],double gnear,double gfar,bool invert ) throw ( cv::Exception ) -{ - cerr<<"MarkerDetector::glGetProjectionMatrix . This a deprecated function. Use CameraParameters::glGetProjectionMatrix instead. "<<__FILE__<<" "<<__LINE__<1) throw cv::Exception(1," min parameter out of range","MarkerDetector::setMinMaxSize",__FILE__,__LINE__); - if (max<=0 || max>1) throw cv::Exception(1," max parameter out of range","MarkerDetector::setMinMaxSize",__FILE__,__LINE__); - if (min>max) throw cv::Exception(1," min>max","MarkerDetector::setMinMaxSize",__FILE__,__LINE__); - _minSize=min; - _maxSize=max; +void MarkerDetector::setMinMaxSize(float min, float max) throw(cv::Exception) { + if (min <= 0 || min > 1) + throw cv::Exception(1, " min parameter out of range", "MarkerDetector::setMinMaxSize", __FILE__, __LINE__); + if (max <= 0 || max > 1) + throw cv::Exception(1, " max parameter out of range", "MarkerDetector::setMinMaxSize", __FILE__, __LINE__); + if (min > max) + throw cv::Exception(1, " min>max", "MarkerDetector::setMinMaxSize", __FILE__, __LINE__); + _minSize = min; + _maxSize = max; } /************************************ @@ -1035,12 +1045,55 @@ void MarkerDetector::setMinMaxSize(float min ,float max )throw(cv::Exception) * ************************************/ -void MarkerDetector::setWarpSize(int val) throw(cv::Exception) -{ - if (val<10) throw cv::Exception(1," invalid canonical image size","MarkerDetector::setWarpSize",__FILE__,__LINE__); - _markerWarpSize = val; +void MarkerDetector::setWarpSize(int val) throw(cv::Exception) { + if (val < 10) + throw cv::Exception(1, " invalid canonical image size", "MarkerDetector::setWarpSize", __FILE__, __LINE__); + _markerWarpSize = val; } -}; +void MarkerDetector::findCornerMaxima(vector< cv::Point2f > &Corners, const cv::Mat &grey, int wsize) { + +// for each element, search in a region around +#pragma omp parallel for + for (size_t i = 0; i < Corners.size(); i++) { + cv::Point2f minLimit(std::max(0, int(Corners[i].x - wsize)), std::max(0, int(Corners[i].y - wsize))); + cv::Point2f maxLimit(std::min(grey.cols, int(Corners[i].x + wsize)), std::min(grey.rows, int(Corners[i].y + wsize))); + + cv::Mat reg = grey(cv::Range(minLimit.y, maxLimit.y), cv::Range(minLimit.x, maxLimit.x)); + cv::Mat harr, harrint; + cv::cornerHarris(reg, harr, 3, 3, 0.04); + + // now, do a sum block operation + cv::integral(harr, harrint); + int bls_a = 4; + for (int y = bls_a; y < harr.rows - bls_a; y++) { + float *h = harr.ptr< float >(y); + for (int x = bls_a; x < harr.cols - bls_a; x++) + h[x] = harrint.at< double >(y + bls_a, x + bls_a) - harrint.at< double >(y + bls_a, x) - harrint.at< double >(y, x + bls_a) + + harrint.at< double >(y, x); + } + + + + cv::Point2f best(-1, -1); + cv::Point2f center(reg.cols / 2, reg.rows / 2); + ; + double maxv = 0; + for (size_t i = 0; i < harr.rows; i++) { + // L1 dist to center + float *har = harr.ptr< float >(i); + for (size_t x = 0; x < harr.cols; x++) { + float d = float(fabs(center.x - x) + fabs(center.y - i)) / float(reg.cols / 2 + reg.rows / 2); + float w = 1. - d; + if (w * har[x] > maxv) { + maxv = w * har[x]; + best = cv::Point2f(x, i); + } + } + } + Corners[i] = best + minLimit; + } +} +}; diff --git a/src/markerdetector.h b/src/markerdetector.h index 63dfcf7..c9bccba 100644 --- a/src/markerdetector.h +++ b/src/markerdetector.h @@ -35,37 +35,35 @@ or implied, of Rafael Muñoz Salinas. #include "marker.h" using namespace std; -namespace aruco -{ +namespace aruco { /**\brief Main class for marker detection * */ -class ARUCO_EXPORTS MarkerDetector -{ - //Represent a candidate to be a maker - class MarkerCandidate : public Marker{ - public: - MarkerCandidate(){} - MarkerCandidate(const Marker &M): Marker(M){} - MarkerCandidate(const MarkerCandidate &M): Marker(M){ - contour=M.contour; - idx=M.idx; - } - MarkerCandidate & operator=(const MarkerCandidate &M){ - (*(Marker*)this)=(*(Marker*)&M); - contour=M.contour; - idx=M.idx; - return *this; - } - - vector contour;//all the points of its contour - int idx;//index position in the global contour list - }; -public: +class ARUCO_EXPORTS MarkerDetector { + // Represent a candidate to be a maker + class MarkerCandidate : public Marker { + public: + MarkerCandidate() {} + MarkerCandidate(const Marker &M) : Marker(M) {} + MarkerCandidate(const MarkerCandidate &M) : Marker(M) { + contour = M.contour; + idx = M.idx; + } + MarkerCandidate &operator=(const MarkerCandidate &M) { + (*(Marker *)this) = (*(Marker *)&M); + contour = M.contour; + idx = M.idx; + return *this; + } + + vector< cv::Point > contour; // all the points of its contour + int idx; // index position in the global contour list + }; + public: /** - * See + * See */ MarkerDetector(); @@ -84,7 +82,8 @@ class ARUCO_EXPORTS MarkerDetector * @param markerSizeMeters size of the marker sides expressed in meters * @param setYPerperdicular If set the Y axis will be perpendicular to the surface. Otherwise, it will be the Z axis */ - void detect(const cv::Mat &input,std::vector &detectedMarkers,cv::Mat camMatrix=cv::Mat(),cv::Mat distCoeff=cv::Mat(),float markerSizeMeters=-1,bool setYPerperdicular=false) throw (cv::Exception); + void detect(const cv::Mat &input, std::vector< Marker > &detectedMarkers, cv::Mat camMatrix = cv::Mat(), cv::Mat distCoeff = cv::Mat(), + float markerSizeMeters = -1, bool setYPerperdicular = false) throw(cv::Exception); /**Detects the markers in the image passed * * If you provide information about the camera parameters and the size of the marker, then, the extrinsics of the markers are detected @@ -95,85 +94,102 @@ class ARUCO_EXPORTS MarkerDetector * @param markerSizeMeters size of the marker sides expressed in meters * @param setYPerperdicular If set the Y axis will be perpendicular to the surface. Otherwise, it will be the Z axis */ - void detect(const cv::Mat &input,std::vector &detectedMarkers, CameraParameters camParams,float markerSizeMeters=-1,bool setYPerperdicular=false) throw (cv::Exception); + void detect(const cv::Mat &input, std::vector< Marker > &detectedMarkers, CameraParameters camParams, float markerSizeMeters = -1, + bool setYPerperdicular = false) throw(cv::Exception); /**This set the type of thresholding methods available */ - enum ThresholdMethods {FIXED_THRES,ADPT_THRES,CANNY}; + enum ThresholdMethods { FIXED_THRES, ADPT_THRES, CANNY }; /**Sets the threshold method */ - void setThresholdMethod(ThresholdMethods m) { - _thresMethod=m; - } + void setThresholdMethod(ThresholdMethods m) { _thresMethod = m; } /**Returns the current threshold method */ - ThresholdMethods getThresholdMethod()const { - return _thresMethod; - } + ThresholdMethods getThresholdMethod() const { return _thresMethod; } /** * Set the parameters of the threshold method * We are currently using the Adptive threshold ee opencv doc of adaptiveThreshold for more info * @param param1: blockSize of the pixel neighborhood that is used to calculate a threshold value for the pixel * @param param2: The constant subtracted from the mean or weighted mean */ - void setThresholdParams(double param1,double param2) { - _thresParam1=param1; - _thresParam2=param2; + void setThresholdParams(double param1, double param2) { + _thresParam1 = param1; + _thresParam2 = param2; } + + /**Allows for a parallel search of several values of the param1 simultaneously (in different threads) + * The param r1 the indicates how many values around the current value of param1 are evaluated. In other words + * if r1>0, param1 is searched in range [param1- r1 ,param1+ r1 ] + * + * r2 unused yet. Added in case of future need. + */ + void setThresholdParamRange(size_t r1 = 0, size_t r2 = 0) { _thresParam1_range = r1; } + /** + * This method assumes that the markers may have some of its corners joined either to another marker + * in a chessboard like pattern) or to a rectangle. This is the case in which the subpixel refinement + * method in opencv work best. + * + * Enabling this does not force you to use locked corners, normals markers will be detected also. However, + * when using locked corners, enabling this option will increase robustness in detection at the cost of + * higher computational time. + * , + * Note for developer: Enabling this option forces a call to findCornerMaxima + */ + void enableLockedCornersMethod(bool enable); + /** * Set the parameters of the threshold method * We are currently using the Adptive threshold ee opencv doc of adaptiveThreshold for more info * param1: blockSize of the pixel neighborhood that is used to calculate a threshold value for the pixel * param2: The constant subtracted from the mean or weighted mean */ - void getThresholdParams(double ¶m1,double ¶m2)const { - param1=_thresParam1; - param2=_thresParam2; + void getThresholdParams(double ¶m1, double ¶m2) const { + param1 = _thresParam1; + param2 = _thresParam2; } /**Returns a reference to the internal image thresholded. It is for visualization purposes and to adjust manually * the parameters */ - const cv::Mat & getThresholdedImage() { - return thres; - } + const cv::Mat &getThresholdedImage() { return thres; } /**Methods for corner refinement */ - enum CornerRefinementMethod {NONE,HARRIS,SUBPIX,LINES}; + enum CornerRefinementMethod { NONE, HARRIS, SUBPIX, LINES }; /** */ - void setCornerRefinementMethod(CornerRefinementMethod method) { - _cornerMethod=method; - } + void setCornerRefinementMethod(CornerRefinementMethod method) { _cornerMethod = method; } /** */ - CornerRefinementMethod getCornerRefinementMethod()const { - return _cornerMethod; - } + CornerRefinementMethod getCornerRefinementMethod() const { return _cornerMethod; } /**Specifies the min and max sizes of the markers as a fraction of the image size. By size we mean the maximum * of cols and rows. * @param min size of the contour to consider a possible marker as valid (0,1] * @param max size of the contour to consider a possible marker as valid [0,1) - * + * */ - void setMinMaxSize(float min=0.03,float max=0.5)throw(cv::Exception); - + void setMinMaxSize(float min = 0.03, float max = 0.5) throw(cv::Exception); + /**reads the min and max sizes employed * @param min output size of the contour to consider a possible marker as valid (0,1] * @param max output size of the contour to consider a possible marker as valid [0,1) - * + * */ - void getMinMaxSize(float &min,float &max){min=_minSize;max=_maxSize;} - - /**Enables/Disables erosion process that is REQUIRED for chessboard like boards. + void getMinMaxSize(float &min, float &max) { + min = _minSize; + max = _maxSize; + } + + /**Deprecated!!! + * + * Enables/Disables erosion process that is REQUIRED for chessboard like boards. * By default, this property is enabled */ - void enableErosion(bool enable){_doErosion=enable;} + void enableErosion(bool enable) {} /** * Specifies a value to indicate the required speed for the internal processes. If you need maximum speed (at the cost of a lower detection rate), @@ -185,20 +201,17 @@ class ARUCO_EXPORTS MarkerDetector void setDesiredSpeed(int val); /** */ - int getDesiredSpeed()const { - return _speed; - } - + int getDesiredSpeed() const { return _speed; } + /** - * Specifies the size for the canonical marker image. A big value makes the detection slower than a small value. + * Specifies the size for the canonical marker image. A big value makes the detection slower than a small value. * Minimun value is 10. Default value is 56. */ - void setWarpSize(int val)throw(cv::Exception);; + void setWarpSize(int val) throw(cv::Exception); + ; /** */ - int getWarpSize()const { - return _markerWarpSize; - } + int getWarpSize() const { return _markerWarpSize; } /** * Allows to specify the function that identifies a marker. Therefore, you can create your own type of markers different from these @@ -212,20 +225,21 @@ class ARUCO_EXPORTS MarkerDetector * * As output your marker function must indicate the following information. First, the output parameter nRotations must indicate how many times the marker * must be rotated clockwise 90 deg to be in its ideal position. (The way you would see it when you print it). This is employed to know - * always which is the corner that acts as reference system. Second, the function must return -1 if the image does not contains one of your markers, and its id otherwise. + * always which is the corner that acts as reference system. Second, the function must return -1 if the image does not contains one of your markers, and its + *id otherwise. * */ - void setMakerDetectorFunction(int (* markerdetector_func)(const cv::Mat &in,int &nRotations) ) { - markerIdDetector_ptrfunc=markerdetector_func; - } + void setMakerDetectorFunction(int (*markerdetector_func)(const cv::Mat &in, int &nRotations)) { markerIdDetector_ptrfunc = markerdetector_func; } - /** Use an smaller version of the input image for marker detection. + /**Deprecated + * + * Use an smaller version of the input image for marker detection. * If your marker is small enough, you can employ an smaller image to perform the detection without noticeable reduction in the precision. * Internally, we are performing a pyrdown operation - * + * * @param level number of times the image size is divided by 2. Internally, we are performing a pyrdown. */ - void pyrDown(unsigned int level){pyrdown_level=level;} + void pyrDown(unsigned int level) {} ///------------------------------------------------- /// Methods you may not need @@ -235,18 +249,16 @@ class ARUCO_EXPORTS MarkerDetector /** * Thesholds the passed image with the specified method. */ - void thresHold(int method,const cv::Mat &grey,cv::Mat &thresImg,double param1=-1,double param2=-1)throw(cv::Exception); + void thresHold(int method, const cv::Mat &grey, cv::Mat &thresImg, double param1 = -1, double param2 = -1) throw(cv::Exception); /** * Detection of candidates to be markers, i.e., rectangles. * This function returns in candidates all the rectangles found in a thresolded image */ - void detectRectangles(const cv::Mat &thresImg,vector > & candidates); + void detectRectangles(const cv::Mat &thresImg, vector< std::vector< cv::Point2f > > &candidates); /**Returns a list candidates to be markers (rectangles), for which no valid id was found after calling detectRectangles */ - const vector > &getCandidates() { - return _candidates; - } + const vector< std::vector< cv::Point2f > > &getCandidates() { return _candidates; } /**Given the iput image with markers, creates an output image with it in the canonical position * @param in input image @@ -255,18 +267,18 @@ class ARUCO_EXPORTS MarkerDetector * @param points 4 corners of the marker in the image in * @return true if the operation succeed */ - bool warp(cv::Mat &in,cv::Mat &out,cv::Size size, std::vector points)throw (cv::Exception); - - - + bool warp(cv::Mat &in, cv::Mat &out, cv::Size size, std::vector< cv::Point2f > points) throw(cv::Exception); + + + /** Refine MarkerCandidate Corner using LINES method * @param candidate candidate to refine corners */ - void refineCandidateLines(MarkerCandidate &candidate, const cv::Mat &camMatrix, const cv::Mat &distCoeff); - - + void refineCandidateLines(MarkerCandidate &candidate, const cv::Mat &camMatrix, const cv::Mat &distCoeff); + + /**DEPRECATED!!! Use the member function in CameraParameters - * + * * Given the intrinsic camera parameters returns the GL_PROJECTION matrix for opengl. * PLease NOTE that when using OpenGL, it is assumed no camera distorsion! So, if it is not true, you should have * undistor image @@ -276,102 +288,105 @@ class ARUCO_EXPORTS MarkerDetector * @param size of the image/window where to render (can be different from the real camera image). Please not that it must be related to CamMatrix * @param proj_matrix output projection matrix to give to opengl * @param gnear,gfar: visible rendering range - * @param invert: indicates if the output projection matrix has to yield a horizontally inverted image because image data has not been stored in the order of glDrawPixels: bottom-to-top. + * @param invert: indicates if the output projection matrix has to yield a horizontally inverted image because image data has not been stored in the order + *of glDrawPixels: bottom-to-top. */ - static void glGetProjectionMatrix( CameraParameters & CamMatrix,cv::Size orgImgSize, cv::Size size,double proj_matrix[16],double gnear,double gfar,bool invert=false )throw(cv::Exception); - -private: + static void glGetProjectionMatrix(CameraParameters &CamMatrix, cv::Size orgImgSize, cv::Size size, double proj_matrix[16], double gnear, double gfar, + bool invert = false) throw(cv::Exception); - bool warp_cylinder ( cv::Mat &in,cv::Mat &out,cv::Size size, MarkerCandidate& mc ) throw ( cv::Exception ); + private: + bool warp_cylinder(cv::Mat &in, cv::Mat &out, cv::Size size, MarkerCandidate &mc) throw(cv::Exception); /** * Detection of candidates to be markers, i.e., rectangles. * This function returns in candidates all the rectangles found in a thresolded image */ - void detectRectangles(const cv::Mat &thresImg,vector & candidates); - //Current threshold method + void detectRectangles(vector< cv::Mat > &vimages, vector< MarkerCandidate > &candidates); + // Current threshold method ThresholdMethods _thresMethod; - //Threshold parameters - double _thresParam1,_thresParam2; - //Current corner method + // Threshold parameters + double _thresParam1, _thresParam2, _thresParam1_range; + // Current corner method CornerRefinementMethod _cornerMethod; - //minimum and maximum size of a contour lenght - float _minSize,_maxSize; - //Speed control + // minimum and maximum size of a contour lenght + float _minSize, _maxSize; + + // is corner locked + bool _useLockedCorners; + + // Speed control int _speed; int _markerWarpSize; - bool _doErosion; - float _borderDistThres;//border around image limits in which corners are not allowed to be detected. - //vectr of candidates to be markers. This is a vector with a set of rectangles that have no valid id - vector > _candidates; - //level of image reduction - int pyrdown_level; - //Images - cv::Mat grey,thres,thres2,reduced; - //pointer to the function that analizes a rectangular region so as to detect its internal marker - int (* markerIdDetector_ptrfunc)(const cv::Mat &in,int &nRotations); + float _borderDistThres; // border around image limits in which corners are not allowed to be detected. + // vectr of candidates to be markers. This is a vector with a set of rectangles that have no valid id + vector< std::vector< cv::Point2f > > _candidates; + // Images + cv::Mat grey, thres; + // pointer to the function that analizes a rectangular region so as to detect its internal marker + int (*markerIdDetector_ptrfunc)(const cv::Mat &in, int &nRotations); /** */ - bool isInto(cv::Mat &contour,std::vector &b); + bool isInto(cv::Mat &contour, std::vector< cv::Point2f > &b); /** */ - int perimeter(std::vector &a); - - -// //GL routines -// -// static void argConvGLcpara2( double cparam[3][4], int width, int height, double gnear, double gfar, double m[16], bool invert )throw(cv::Exception); -// static int arParamDecompMat( double source[3][4], double cpara[3][4], double trans[3][4] )throw(cv::Exception); -// static double norm( double a, double b, double c ); -// static double dot( double a1, double a2, double a3, -// double b1, double b2, double b3 ); -// - - //detection of the - void findBestCornerInRegion_harris(const cv::Mat & grey,vector & Corners,int blockSize); - - + int perimeter(std::vector< cv::Point2f > &a); + + + // //GL routines + // + // static void argConvGLcpara2( double cparam[3][4], int width, int height, double gnear, double gfar, double m[16], bool invert )throw(cv::Exception); + // static int arParamDecompMat( double source[3][4], double cpara[3][4], double trans[3][4] )throw(cv::Exception); + // static double norm( double a, double b, double c ); + // static double dot( double a1, double a2, double a3, + // double b1, double b2, double b3 ); + // + + // detection of the + void findBestCornerInRegion_harris(const cv::Mat &grey, vector< cv::Point2f > &Corners, int blockSize); + + // auxiliar functions to perform LINES refinement - void interpolate2Dline( const vector< cv::Point2f > &inPoints, cv::Point3f &outLine); - cv::Point2f getCrossPoint(const cv::Point3f& line1, const cv::Point3f& line2); - void distortPoints(vector in, vector &out, const cv::Mat &camMatrix, const cv::Mat &distCoeff); - - - /**Given a vector vinout with elements and a boolean vector indicating the lements from it to remove, + void interpolate2Dline(const vector< cv::Point2f > &inPoints, cv::Point3f &outLine); + cv::Point2f getCrossPoint(const cv::Point3f &line1, const cv::Point3f &line2); + void distortPoints(vector< cv::Point2f > in, vector< cv::Point2f > &out, const cv::Mat &camMatrix, const cv::Mat &distCoeff); + + + /**Given a vector vinout with elements and a boolean vector indicating the lements from it to remove, * this function remove the elements * @param vinout * @param toRemove */ - template - void removeElements(vector & vinout,const vector &toRemove) - { - //remove the invalid ones by setting the valid in the positions left by the invalids - size_t indexValid=0; - for (size_t i=0;i void removeElements(vector< T > &vinout, const vector< bool > &toRemove) { + // remove the invalid ones by setting the valid in the positions left by the invalids + size_t indexValid = 0; + for (size_t i = 0; i < toRemove.size(); i++) { + if (!toRemove[i]) { + if (indexValid != i) + vinout[indexValid] = vinout[i]; + indexValid++; + } } - } - vinout.resize(indexValid); - } - - //graphical debug - void drawApproxCurve(cv::Mat &in,std::vector &approxCurve ,cv::Scalar color); - void drawContour(cv::Mat &in,std::vector &contour,cv::Scalar ); - void drawAllContours(cv::Mat input, std::vector > &contours); - void draw(cv::Mat out,const std::vector &markers ); - - - template void joinVectors(vector > &vv,vector &v,bool clearv=false){ - if (clearv) v.clear(); - for(size_t i=0;i &approxCurve, cv::Scalar color); + void drawContour(cv::Mat &in, std::vector< cv::Point > &contour, cv::Scalar); + void drawAllContours(cv::Mat input, std::vector< std::vector< cv::Point > > &contours); + void draw(cv::Mat out, const std::vector< Marker > &markers); + // method to refine corner detection in case the internal border after threshold is found + // This was tested in the context of chessboard methods + void findCornerMaxima(vector< cv::Point2f > &Corners, const cv::Mat &grey, int wsize); + template < typename T > void joinVectors(vector< vector< T > > &vv, vector< T > &v, bool clearv = false) { + if (clearv) + v.clear(); + for (size_t i = 0; i < vv.size(); i++) + for (size_t j = 0; j < vv[i].size(); j++) + v.push_back(vv[i][j]); + } +}; }; #endif diff --git a/src/subpixelcorner.cpp b/src/subpixelcorner.cpp index f53620f..2abc4ec 100644 --- a/src/subpixelcorner.cpp +++ b/src/subpixelcorner.cpp @@ -2,210 +2,186 @@ #include using namespace cv; -namespace aruco{ - -SubPixelCorner::SubPixelCorner() -{ - _winSize=15; - _apertureSize=3; - _term.maxCount=10; - _term.epsilon=0.1; - _term.type=CV_TERMCRIT_ITER | CV_TERMCRIT_EPS; - enable=true; +namespace aruco { + +SubPixelCorner::SubPixelCorner() { + _winSize = 15; + _apertureSize = 3; + _term.maxCount = 10; + _term.epsilon = 0.1; + _term.type = CV_TERMCRIT_ITER | CV_TERMCRIT_EPS; + enable = true; } -void SubPixelCorner::checkTerm() -{ - switch( _term.type) - { +void SubPixelCorner::checkTerm() { + switch (_term.type) { case CV_TERMCRIT_ITER: _term.epsilon = 0.f; _term.maxCount; break; case CV_TERMCRIT_EPS: - _term.maxCount=_term.COUNT; + _term.maxCount = _term.COUNT; break; case CV_TERMCRIT_ITER | CV_TERMCRIT_EPS: break; default: - _term.maxCount=_term.COUNT; - _term.epsilon=0.1; - _term.type=CV_TERMCRIT_ITER | CV_TERMCRIT_EPS; - break; + _term.maxCount = _term.COUNT; + _term.epsilon = 0.1; + _term.type = CV_TERMCRIT_ITER | CV_TERMCRIT_EPS; + break; } - eps = std::max( _term.epsilon ,0.0); - eps=eps*eps; - - _max_iters= std::max( _term.maxCount, 1 ); - int max1=TermCriteria::MAX_ITER; - _max_iters= std::min(_max_iters,max1); + eps = std::max(_term.epsilon, 0.0); + eps = eps * eps; + _max_iters = std::max(_term.maxCount, 1); + int max1 = TermCriteria::MAX_ITER; + _max_iters = std::min(_max_iters, max1); } -double SubPixelCorner::pointDist(cv::Point2f estimate_corner,cv::Point2f curr_corner) -{ - double dist=((curr_corner.x-estimate_corner.x)*(curr_corner.x-estimate_corner.x))+ - ((curr_corner.y-estimate_corner.y)*(curr_corner.y-estimate_corner.y)); +double SubPixelCorner::pointDist(cv::Point2f estimate_corner, cv::Point2f curr_corner) { + double dist = ((curr_corner.x - estimate_corner.x) * (curr_corner.x - estimate_corner.x)) + + ((curr_corner.y - estimate_corner.y) * (curr_corner.y - estimate_corner.y)); return dist; } -void SubPixelCorner::generateMask() -{ +void SubPixelCorner::generateMask() { - double coeff = 1. / (_winSize*_winSize); - float * maskX=(float *)calloc(1,(_winSize*sizeof(float))); - float * maskY=(float *)calloc(1,(_winSize*sizeof(float))); - mask.create (_winSize,_winSize,CV_32FC(1)); + double coeff = 1. / (_winSize * _winSize); + float *maskX = (float *)calloc(1, (_winSize * sizeof(float))); + float *maskY = (float *)calloc(1, (_winSize * sizeof(float))); + mask.create(_winSize, _winSize, CV_32FC(1)); /* calculate mask */ - int k=0; - for( int i = -_winSize/2, k = 0; i <= _winSize/2; i++, k++ ) - { - maskX[k] = (float)exp( -i * i * coeff ); - + int k = 0; + for (int i = -_winSize / 2, k = 0; i <= _winSize / 2; i++, k++) { + maskX[k] = (float)exp(-i * i * coeff); } - maskY = maskX; + maskY = maskX; - for( int i = 0; i < _winSize; i++ ) - { - float * mask_ptr=mask.ptr (i); - for( int j = 0; j < _winSize; j++ ) - { + for (int i = 0; i < _winSize; i++) { + float *mask_ptr = mask.ptr< float >(i); + for (int j = 0; j < _winSize; j++) { mask_ptr[j] = maskX[j] * maskY[i]; } } - } -void SubPixelCorner::RefineCorner(cv::Mat image,std::vector &corners) -{ +void SubPixelCorner::RefineCorner(cv::Mat image, std::vector< cv::Point2f > &corners) { - if(enable==false) + if (enable == false) return; checkTerm(); - generateMask (); - //loop over all the corner points - for(int k=0;kimage.rows || estimate_corner.y > image.cols) + if (estimate_corner.x < 0 || estimate_corner.y < 0 || estimate_corner.y > image.rows || estimate_corner.y > image.cols) continue; - int iter=0; - double dist=TermCriteria::EPS; - //loop till termination criteria is met - do - { - iter=iter+1; - curr_corner=estimate_corner; - - /* -Point cx; -cx.x=floor(curr_corner.x); -cx.y=floor(curr_corner.y); -double dx=curr_corner.x-cx.x; -double dy=curr_corner.y-cx.y; -float vIx[2]; -float vIy[2]; - -vIx[0] = dx; -vIx[1] = 1 - dx; -vIy[0] = dy; -vIy[1] = 1 - dy; - -int x1=std::max((int)(cx.x-_winSize-_apertureSize/2),0); -int y1=std::max((int)(cx.y-_winSize-_apertureSize/2),0); - -xmin = x1<0?0:x1; -xmax = x1+_winSize(i); - float *dy_ptr=Dy.ptr (i); - ly=i-_winSize/2-_apertureSize/2; - - float * mask_ptr=mask.ptr (ly+_winSize/2); - - for(int j=_apertureSize/2;j<=_winSize;j++) - { - - lx=j-_winSize/2-_apertureSize/2; - //cerr << lx+_winSize/2 << ":" ; - double val=mask_ptr[lx+_winSize/2]; - double dxx=dx_ptr[j]*dx_ptr[j]*val; - double dyy=dy_ptr[j]*dy_ptr[j]*val; - double dxy=dx_ptr[j]*dy_ptr[j]*val; - - A=A+dxx; - B=B+dxy; - E=E+dyy; - C=C+dxx*lx+dxy*ly; - F=F+dxy*lx+dyy*ly; - + int iter = 0; + double dist = TermCriteria::EPS; + // loop till termination criteria is met + do { + iter = iter + 1; + curr_corner = estimate_corner; + + /* + Point cx; + cx.x=floor(curr_corner.x); + cx.y=floor(curr_corner.y); + double dx=curr_corner.x-cx.x; + double dy=curr_corner.y-cx.y; + float vIx[2]; + float vIy[2]; + + vIx[0] = dx; + vIx[1] = 1 - dx; + vIy[0] = dy; + vIy[1] = 1 - dy; + + int x1=std::max((int)(cx.x-_winSize-_apertureSize/2),0); + int y1=std::max((int)(cx.y-_winSize-_apertureSize/2),0); + + xmin = x1<0?0:x1; + xmax = x1+_winSize(i); + float *dy_ptr = Dy.ptr< float >(i); + ly = i - _winSize / 2 - _apertureSize / 2; + + float *mask_ptr = mask.ptr< float >(ly + _winSize / 2); + + for (int j = _apertureSize / 2; j <= _winSize; j++) { + + lx = j - _winSize / 2 - _apertureSize / 2; + // cerr << lx+_winSize/2 << ":" ; + double val = mask_ptr[lx + _winSize / 2]; + double dxx = dx_ptr[j] * dx_ptr[j] * val; + double dyy = dy_ptr[j] * dy_ptr[j] * val; + double dxy = dx_ptr[j] * dy_ptr[j] * val; + + A = A + dxx; + B = B + dxy; + E = E + dyy; + C = C + dxx * lx + dxy * ly; + F = F + dxy * lx + dyy * ly; + } } - } - //computing denominator - double det=(A*E-B*B); - if(fabs( det ) > DBL_EPSILON*DBL_EPSILON) - { - det=1.0/det; - //translating back to original corner and adding new estimates - estimate_corner.x=curr_corner.x+((C*E)-(B*F))*det; - estimate_corner.y=curr_corner.y+((A*F)-(C*D))*det; - } - else - { - estimate_corner.x=curr_corner.x; - estimate_corner.y=curr_corner.y; - } + // computing denominator + double det = (A * E - B * B); + if (fabs(det) > DBL_EPSILON * DBL_EPSILON) { + det = 1.0 / det; + // translating back to original corner and adding new estimates + estimate_corner.x = curr_corner.x + ((C * E) - (B * F)) * det; + estimate_corner.y = curr_corner.y + ((A * F) - (C * D)) * det; + } else { + estimate_corner.x = curr_corner.x; + estimate_corner.y = curr_corner.y; + } - dist=pointDist(estimate_corner,curr_corner); + dist = pointDist(estimate_corner, curr_corner); - }while(iter<_max_iters && dist>eps); + } while (iter < _max_iters && dist > eps); - //double dist=pointDist(corners[k],estimate_corner); - if(fabs(corners[k].x-estimate_corner.x) > _winSize || fabs(corners[k].y-estimate_corner.y)>_winSize) - { - estimate_corner.x=corners[k].x; - estimate_corner.y=corners[k].y; + // double dist=pointDist(corners[k],estimate_corner); + if (fabs(corners[k].x - estimate_corner.x) > _winSize || fabs(corners[k].y - estimate_corner.y) > _winSize) { + estimate_corner.x = corners[k].x; + estimate_corner.y = corners[k].y; } - corners[k].x=estimate_corner.x; - corners[k].y=estimate_corner.y; - //cerr << "EEE" << corners[k].x <<":" << corners[k].y << endl; - + corners[k].x = estimate_corner.x; + corners[k].y = estimate_corner.y; + // cerr << "EEE" << corners[k].x <<":" << corners[k].y << endl; } - } - } diff --git a/src/subpixelcorner.h b/src/subpixelcorner.h index 2ec6ebe..7beb420 100644 --- a/src/subpixelcorner.h +++ b/src/subpixelcorner.h @@ -1,39 +1,33 @@ #ifndef aruco_SUBPIXELCORNER_HPP -#define aruco_SUBPIXELCORNER_HPP +#define aruco_SUBPIXELCORNER_HPP #include // Basic OpenCV structures (cv::Mat) -namespace aruco -{ +namespace aruco { -class SubPixelCorner -{ -private: +class SubPixelCorner { + private: int _winSize; int _apertureSize; cv::TermCriteria _term; double eps; cv::Mat mask; int _max_iters; -public: + + public: bool enable; SubPixelCorner(); void checkTerm(); - double pointDist(cv::Point2f estimate_corner,cv::Point2f curr_corner); + double pointDist(cv::Point2f estimate_corner, cv::Point2f curr_corner); - ///method to refine the corners - void RefineCorner(cv::Mat image,std::vector &corners); + /// method to refine the corners + void RefineCorner(cv::Mat image, std::vector< cv::Point2f > &corners); - //function to generate the mask + // function to generate the mask void generateMask(); - - }; - - - } #endif // SUBPIXELCORNER_HPP diff --git a/utils/CMakeLists.txt b/utils/CMakeLists.txt index e55a23a..2bc31c8 100644 --- a/utils/CMakeLists.txt +++ b/utils/CMakeLists.txt @@ -31,5 +31,8 @@ IF(GL_FOUND) ADD_EXECUTABLE(aruco_test_board_gl aruco_test_board_gl.cpp) TARGET_LINK_LIBRARIES(aruco_test_board_gl ${OPENGL_LIBS}) - install_executable( aruco_test_gl aruco_test_board_gl ) + ADD_EXECUTABLE(aruco_test_board_gl_mask aruco_test_board_gl_mask.cpp) + TARGET_LINK_LIBRARIES(aruco_test_board_gl_mask ${OPENGL_LIBS}) + + install_executable( aruco_test_gl aruco_test_board_gl aruco_test_board_gl_mask ) ENDIF(GL_FOUND) diff --git a/utils/aruco_board_pix2meters.cpp b/utils/aruco_board_pix2meters.cpp index ac418e8..d8a2ceb 100644 --- a/utils/aruco_board_pix2meters.cpp +++ b/utils/aruco_board_pix2meters.cpp @@ -26,38 +26,43 @@ authors and should not be interpreted as representing official policies, either or implied, of Rafael Muñoz Salinas. ********************************/ -//This program converts a boardconfiguration file expressed in pixel to another one expressed in meters +// This program converts a boardconfiguration file expressed in pixel to another one expressed in meters #include #include "board.h" using namespace std; using namespace aruco; -int main(int argc,char **argv){ - try{ - - if (argc<4){ - cerr<<"Usage: in_boardConfiguration.yml markerSize_meters out_boardConfiguration.yml"< #include #include +#include cv::VideoCapture TheVideoCapturer; @@ -16,72 +17,70 @@ aruco::BoardConfiguration TheBoardConfig; aruco::BoardDetector TheBoardDetector; cv::Mat TheInputImage; aruco::CameraParameters TheCamParams; -std::vector The_rvecs,The_tvecs;//locations at which calibration is done -std::vector > TheimagePointsV; -std::vector > TheobjPointsV; //image and obj points at which calibration is done -float ViewPointDistance=0.025; +std::vector< cv::Mat > The_rvecs, The_tvecs; // locations at which calibration is done +std::vector< vector< cv::Point2f > > TheimagePointsV; +std::vector< vector< cv::Point3f > > TheobjPointsV; // image and obj points at which calibration is done +float ViewPointDistance = 0.025; using namespace std; -//returns an initial dummy camera parameters +// returns an initial dummy camera parameters aruco::CameraParameters getDummyInitialCameraParams(cv::Size imageSize) { aruco::CameraParameters Cp; - cv::Mat CamMatrix=cv::Mat::eye(3,3,CV_32F); - CamMatrix.at(0,0)=500; - CamMatrix.at(1,1)=500; - CamMatrix.at(0,2)=imageSize.width/2; - CamMatrix.at(1,2)=imageSize.height/2; - Cp.setParams(CamMatrix,cv::Mat::zeros(1,5,CV_32F) ,imageSize); + cv::Mat CamMatrix = cv::Mat::eye(3, 3, CV_32F); + CamMatrix.at< float >(0, 0) = 500; + CamMatrix.at< float >(1, 1) = 500; + CamMatrix.at< float >(0, 2) = imageSize.width / 2; + CamMatrix.at< float >(1, 2) = imageSize.height / 2; + Cp.setParams(CamMatrix, cv::Mat::zeros(1, 5, CV_32F), imageSize); return Cp; - } -//computes minimum distance of T to elements in The_tvecs +// computes minimum distance of T to elements in The_tvecs float minDistanceToStoredLocations(cv::Mat T) { - //normalize R - float minDist=std::numeric_limits< float >::max(); - for (size_t i=0;i::max(); + for (size_t i = 0; i < The_tvecs.size(); i++) { + // first, normalize the vector of R + + float norm = cv::norm(T - The_tvecs[i]); + if (norm < minDist) + minDist = norm; + // now, check norm } return minDist; } // inclusion of new points to compute -void setCurrentViewPoint(vector &objPoints,vector & imagePoints) { - cv::Mat Rvec,Tvec; - //get the current location given the points - cv::solvePnP(objPoints,imagePoints, TheCamParams.CameraMatrix,TheCamParams.Distorsion,Rvec,Tvec); - //check the minimum distance to the rest of valid locations -// cerr<<"Tvec="< ViewPointDistance ) { - cerr<<"Adding new view point"< &objPoints, vector< cv::Point2f > &imagePoints) { + cv::Mat Rvec, Tvec; + // get the current location given the points + cv::solvePnP(objPoints, imagePoints, TheCamParams.CameraMatrix, TheCamParams.Distorsion, Rvec, Tvec); + // check the minimum distance to the rest of valid locations + // cerr<<"Tvec="< ViewPointDistance) { + cerr << "Adding new view point" << endl; + // addToPool(imagePoints,objPoints); + // add the points to calculate the camera params TheimagePointsV.push_back(imagePoints); TheobjPointsV.push_back(objPoints); The_tvecs.push_back(Tvec); The_rvecs.push_back(Rvec); -// char c;cin>>c; - if (TheobjPointsV.size()>=3 && TheobjPointsV.size()<8) { - //now, calibrate camera - float repro=cv::calibrateCamera(TheobjPointsV,TheimagePointsV,TheCamParams.CamSize, TheCamParams.CameraMatrix, TheCamParams.Distorsion,The_rvecs,The_tvecs); - cerr<<"Recalibared: "<>c; + if (TheobjPointsV.size() >= 3 && TheobjPointsV.size() < 8) { + // now, calibrate camera + float repro = cv::calibrateCamera(TheobjPointsV, TheimagePointsV, TheCamParams.CamSize, TheCamParams.CameraMatrix, TheCamParams.Distorsion, + The_rvecs, The_tvecs); + cerr << "Recalibared: " << repro << endl; + cerr << TheCamParams.CameraMatrix << " " << endl << TheCamParams.Distorsion << endl; } - - } - } // void getObjectAndImagePoints(aruco::Board &B, cv::Mat &objPoints,cv::Mat &imagePoints) { // //composes the matrices // int nPoints=B.size()*4; -// +// // imagePoints.create(nPoints,1,CV_32FC2); // objPoints.create(nPoints,1,CV_32FC3); // int cIdx=0; @@ -92,115 +91,115 @@ void setCurrentViewPoint(vector &objPoints,vector & im // objPoints.ptr(0)[cIdx]= mInfo[j]; // } // } -// +// // } -void getObjectAndImagePoints(aruco::Board &B, vector &objPoints,vector &imagePoints) { - //composes the matrices - int nPoints=B.size()*4; - - int cIdx=0; - for (size_t i=0;i &objPoints, vector< cv::Point2f > &imagePoints) { + // composes the matrices + int nPoints = B.size() * 4; + + int cIdx = 0; + for (size_t i = 0; i < B.size(); i++) { + const aruco::MarkerInfo &mInfo = B.conf.getMarkerInfo(B[i].id); + for (int j = 0; j < 4; j++, cIdx++) { imagePoints.push_back(B[i][j]); - objPoints.push_back( mInfo[j]); + objPoints.push_back(mInfo[j]); } } - } -int main(int argc,char **argv) { +int main(int argc, char **argv) { try { - if (argc<4) { - cerr<<"Usage : (in.avi|live) board_in_meters.yml out_camera_params.yml [viewPointDist (default 0.025)]"; + if (argc < 4) { + cerr << "Usage : (in.avi|live) board_in_meters.yml out_camera_params.yml [viewPointDist (default 0.025)]\n"; return -1; } - //extra param? - if (argc>=5) { - ViewPointDistance=atof(argv[4]); - cerr<<"Using view point distance="<= 5) { + ViewPointDistance = atof(argv[4]); + cerr << "Using view point distance=" << ViewPointDistance << endl; } - //Open video stream - if (string(argv[1])=="live") + // Open video stream + if (string(argv[1]) == "live") TheVideoCapturer.open(0); - else TheVideoCapturer.open(argv[1]); + else + TheVideoCapturer.open(argv[1]); if (!TheVideoCapturer.isOpened()) { - cerr<<"Could not open video"<0.2) { - //lets add the detection - vector objPoints; - vector imgPoints; - getObjectAndImagePoints(TheBoardDetector.getDetectedBoard(),objPoints,imgPoints); - setCurrentViewPoint(objPoints,imgPoints); + // detect + float prob = TheBoardDetector.detect(TheInputImage); + for (size_t i = 0; i < TheBoardDetector.getDetectedBoard().size(); i++) + TheBoardDetector.getDetectedBoard()[i].draw(TheInputImage, cv::Scalar(0, 0, 255)); + cv::imshow("image", TheInputImage); + key = cv::waitKey(10); + if (prob > 0.2) { + // lets add the detection + vector< cv::Point3f > objPoints; + vector< cv::Point2f > imgPoints; + getObjectAndImagePoints(TheBoardDetector.getDetectedBoard(), objPoints, imgPoints); + setCurrentViewPoint(objPoints, imgPoints); } - } -//do the final calibration with all images - if (TheobjPointsV.size()>=3) { - cerr<<"Performing final calibration with the "<< TheobjPointsV.size()<<" images selected. It migth take a little while"< reprj; - int nToRemove=0,nTotal=0; - vector > toRemove(TheobjPointsV.size()); - for(size_t i=0;i0.99){ - toRemove[i][j]=true; - nToRemove++; - } - } - } - cout<<"Total o "< > ob3d(TheobjPointsV.size()); - vector > ob2d(TheobjPointsV.size()); - for(size_t i=0;i= 3) { + cerr << "Performing final calibration with the " << TheobjPointsV.size() << " images selected. It migth take a little while" << endl; + float repro = cv::calibrateCamera(TheobjPointsV, TheimagePointsV, TheCamParams.CamSize, TheCamParams.CameraMatrix, TheCamParams.Distorsion, + The_rvecs, The_tvecs); + cerr << "Recalibared: " << repro << endl; + cerr << TheCamParams.CameraMatrix << " " << endl << TheCamParams.Distorsion << endl; + // do a refinement using these with low reprjection error + vector< cv::Point2f > reprj; + int nToRemove = 0, nTotal = 0; + vector< vector< bool > > toRemove(TheobjPointsV.size()); + for (size_t i = 0; i < TheobjPointsV.size(); i++) { + cv::projectPoints(TheobjPointsV[i], The_rvecs[i], The_tvecs[i], TheCamParams.CameraMatrix, TheCamParams.Distorsion, reprj); + toRemove[i].resize(TheobjPointsV[i].size(), false); + for (size_t j = 0; j < TheimagePointsV.size(); j++, nTotal++) { + if (cv::norm(reprj[j] - TheimagePointsV[i][j]) > 0.99) { + toRemove[i][j] = true; + nToRemove++; + } + } + } + cout << "Total o " << nToRemove << " outliers of " << nTotal << " total points" << endl; + // now, copy only the good ones + vector< vector< cv::Point3f > > ob3d(TheobjPointsV.size()); + vector< vector< cv::Point2f > > ob2d(TheobjPointsV.size()); + for (size_t i = 0; i < TheobjPointsV.size(); i++) { + for (size_t j = 0; j < TheimagePointsV[i].size(); j++) { + if (!toRemove[i][j]) { + ob3d[i].push_back(TheobjPointsV[i][j]); + ob2d[i].push_back(TheimagePointsV[i][j]); + } + } + } + + // repeating calibration without outliers + cerr << "Performing final calibration with the " << TheobjPointsV.size() << " images selected. It migth take a little while" << endl; + repro = cv::calibrateCamera(ob3d, ob2d, TheCamParams.CamSize, TheCamParams.CameraMatrix, TheCamParams.Distorsion, The_rvecs, The_tvecs); + cerr << "Recalibared: " << repro << endl; + cerr << TheCamParams.CameraMatrix << " " << endl << TheCamParams.Distorsion << endl; + TheCamParams.saveToFile(argv[3]); } @@ -208,6 +207,6 @@ int main(int argc,char **argv) { } catch (std::exception &ex) { - cout<= 5) + pixSize = atoi(argv[4]); + if (argc >= 6) + typeBoard = atoi(argv[5]); + if (argc >= 7) + interMarkerDistance = atof(argv[6]); + if ((interMarkerDistance > 1.f) || (interMarkerDistance < 0.f)) { + cerr << "Incorrect interMarkerDistance '" << interMarkerDistance << "' -- needs to be [0,1]" << endl; return -1; } - int pixSize=100; - float interMarkerDistance=0.2; - bool isChessBoard=false; - int typeBoard=0; - if (argc>=5) pixSize=atoi(argv[4]); - if (argc>=6) typeBoard=atoi(argv[5]); - if (argc>=7) interMarkerDistance=atoi(argv[6]); aruco::BoardConfiguration BInfo; Mat BoardImage; - if (typeBoard==0) - BoardImage=aruco::FiducidalMarkers::createBoardImage(Size(XSize,YSize), pixSize,pixSize*interMarkerDistance,BInfo); - else if (typeBoard==1) - BoardImage=aruco::FiducidalMarkers::createBoardImage_ChessBoard(Size(XSize,YSize), pixSize,BInfo); - else if (typeBoard==2) - BoardImage=aruco::FiducidalMarkers::createBoardImage_Frame(Size(XSize,YSize), pixSize,pixSize*interMarkerDistance,BInfo); - - else {cerr<<"Incorrect board type"< outfile.jpg sizeInPixels"< outfile.(jpg|png|ppm|bmp) [sizeInPixels:500 default] [locked (0,1) : 0 default]" << endl; + return -1; + } + int pixSize = 500; + if (argc >= 4) + pixSize = atoi(argv[3]); + bool locked = false; + if (argc >= 5) + locked = atoi(argv[4]); + Mat marker = aruco::FiducidalMarkers::createMarkerImage(atoi(argv[1]), pixSize, true, locked); + + cv::imwrite(argv[2], marker); + + } catch (std::exception &ex) { + cout << ex.what() << endl; + } +} diff --git a/utils/aruco_selectoptimalmarkers.cpp b/utils/aruco_selectoptimalmarkers.cpp index 239b076..ba648b9 100644 --- a/utils/aruco_selectoptimalmarkers.cpp +++ b/utils/aruco_selectoptimalmarkers.cpp @@ -35,169 +35,175 @@ or implied, of Rafael Muñoz Salinas. #include #include "aruco.h" #include +#include #include "arucofidmarkers.h" using namespace cv; using namespace std; -int HammDist_(const cv::Mat &m1,const cv::Mat & m2) -{ - - int dist=0; - for (int y=0;y<5;y++) - for (int x=0;x<5;x++) - if (m1.at(y,x)!=m2.at(y,x)) dist++; - return dist; +int HammDist_(const cv::Mat &m1, const cv::Mat &m2) { + int dist = 0; + for (int y = 0; y < 5; y++) + for (int x = 0; x < 5; x++) + if (m1.at< uchar >(y, x) != m2.at< uchar >(y, x)) + dist++; + return dist; } -Mat rotate(Mat in) -{ +Mat rotate(Mat in) { Mat out; in.copyTo(out); - for (int i=0;i(i,j)=in.at(in.cols-j-1,i); + for (int i = 0; i < in.rows; i++) { + for (int j = 0; j < in.cols; j++) { + out.at< uchar >(i, j) = in.at< uchar >(in.cols - j - 1, i); } } return out; } -int HammDist(const cv::Mat &m1,const cv::Mat & m2) -{ - cv::Mat mc=m1.clone(); - int minD=std::numeric_limits::max(); - for(int i=0;i<4;i++){ - int dist=HammDist_(mc,m2); - if( dist::max(); + for (int i = 0; i < 4; i++) { + int dist = HammDist_(mc, m2); + if (dist < minD) + minD = dist; + mc = rotate(mc); } return minD; - } -int entropy(const cv::Mat &marker) -{ - - //the entropy is calcualte for each bin as the number of elements different from it in its sourroundings - int totalEntropy=0; - for (int y=0;y<5;y++) - for (int x=0;x<5;x++){ - int minX=max(x-1,0); - int maxX=min(x+1,5); - int minY=max(y-1,0); - int maxY=min(y+1,5); - - for(int yy=minY;yy(y,x)!=marker.at(yy,xx)) totalEntropy++; - } - +int entropy(const cv::Mat &marker) { + + // the entropy is calcualte for each bin as the number of elements different from it in its sourroundings + int totalEntropy = 0; + for (int y = 0; y < 5; y++) + for (int x = 0; x < 5; x++) { + int minX = max(x - 1, 0); + int maxX = min(x + 1, 5); + int minY = max(y - 1, 0); + int maxY = min(y + 1, 5); + + for (int yy = minY; yy < maxY; yy++) + for (int xx = minX; xx < maxX; xx++) + if (marker.at< uchar >(y, x) != marker.at< uchar >(yy, xx)) + totalEntropy++; + } + return totalEntropy; } +// return the index of the param or -1 if not found +int findParam(std::string param, int argc, char *argv[]) { + for (int i = 0; i < argc; i++) + if (string(argv[i]) == param) + return i; -int main(int argc,char **argv) -{ + return -1; +} + +int main(int argc, char **argv) { try { - if (argc<4) { + if (argc < 4) { - //You can also use ids 2000-2007 but it is not safe since there are a lot of false positives. - cerr<<"Usage: nofMarkers outbasename size [minimum_entropy(9,25)]"<=5) minimimEntropy=atoi(argv[4]); - vector markers; - vector ventropy; - for (int i=0;i<1024;i++){ - markers.push_back(aruco::FiducidalMarkers::getMarkerMat(i) ); - ventropy.push_back(entropy( aruco::FiducidalMarkers::getMarkerMat(i) )); - } - cout<<"Calculating distance matrix"<(i,j)=distances.at(j,i)= HammDist ( markers[i],markers[j]); - cout<<"done"< usedMarkers(1024,false); - - - - vector selectedMarkers; - //select the masker with higher entropy first - int bestEntr=0; - for(size_t i=0;iventropy[bestEntr]) bestEntr=i; + + bool locked = findParam("-locked", argc, argv) != -1; // if found, true + + // create a vector with all markers + int minimimEntropy = 0; + if (argc >= 5) + minimimEntropy = atoi(argv[4]); + vector< cv::Mat > markers; + vector< int > ventropy; + for (int i = 0; i < 1024; i++) { + markers.push_back(aruco::FiducidalMarkers::getMarkerMat(i)); + ventropy.push_back(entropy(aruco::FiducidalMarkers::getMarkerMat(i))); + } + cout << "Calculating distance matrix" << endl; + // create a matrix with all distances + cv::Mat distances = cv::Mat::zeros(1024, 1024, CV_32SC1); + for (int i = 0; i < 1024; i++) + for (int j = i + 1; j < 1024; j++) + distances.at< int >(i, j) = distances.at< int >(j, i) = HammDist(markers[i], markers[j]); + cout << "done" << endl; + // + int nMarkers = atoi(argv[1]); + // select the first marker + vector< bool > usedMarkers(1024, false); + + + + vector< int > selectedMarkers; + // select the masker with higher entropy first + int bestEntr = 0; + for (size_t i = 0; i < ventropy.size(); i++) + if (ventropy[i] > ventropy[bestEntr]) + bestEntr = i; selectedMarkers.push_back(bestEntr); - usedMarkers[bestEntr]=true; - - //remove these with low entropy. Not very elegnat. Other day I will improve the algorithm - //to make it multiobjective - for(size_t i=0;i::max(); - for (size_t k=0;k ( selectedMarkers[k], j) ( selectedMarkers[k], j); -// cout<<"j="<shorterDist){ - shorterDist=minDist; - bestMarker=j; - } + int minDist = std::numeric_limits< int >::max(); + for (size_t k = 0; k < selectedMarkers.size(); k++) + if (distances.at< int >(selectedMarkers[k], j) < minDist) + minDist = distances.at< int >(selectedMarkers[k], j); + // cout<<"j="< shorterDist) { + shorterDist = minDist; + bestMarker = j; + } } } - if (bestMarker!=-1 && shorterDist>1 ){ - selectedMarkers.push_back(bestMarker); - usedMarkers[bestMarker]=true; -// cout<<"Best id="<>c; + if (bestMarker != -1 && shorterDist > 1) { + selectedMarkers.push_back(bestMarker); + usedMarkers[bestMarker] = true; + // cout<<"Best id="<>c; } - - sort(selectedMarkers.begin(),selectedMarkers.end()); - for(size_t i=0;i::max(); - for(size_t i=0;i"< ( selectedMarkers[i], selectedMarkers[j])< ( selectedMarkers[i], selectedMarkers[j]) < minDist) minDist=distances.at ( selectedMarkers[i], selectedMarkers[j]); - - } - - - cout<<"Min Dist="<::max(); + for (size_t i = 0; i < selectedMarkers.size() - 1; i++) + for (size_t j = i + 1; j < selectedMarkers.size(); j++) { + // cout<<" d=" << selectedMarkers[i]<<" "<"< ( selectedMarkers[i], selectedMarkers[j])<(selectedMarkers[i], selectedMarkers[j]) < minDist) + minDist = distances.at< int >(selectedMarkers[i], selectedMarkers[j]); + } -} + cout << "Min Dist=" << minDist << endl; + + } catch (std::exception &ex) { + cout << ex.what() << endl; + } +} diff --git a/utils/aruco_simple.cpp b/utils/aruco_simple.cpp index 7df7386..cc7a51b 100644 --- a/utils/aruco_simple.cpp +++ b/utils/aruco_simple.cpp @@ -25,7 +25,7 @@ The views and conclusions contained in the software and documentation are those authors and should not be interpreted as representing official policies, either expressed or implied, of Rafael Muñoz Salinas. ********************************/ - + #include #include "aruco.h" @@ -33,72 +33,71 @@ or implied, of Rafael Muñoz Salinas. #include using namespace cv; using namespace aruco; -int main(int argc,char **argv) -{ - try - { - if (argc<2) { - cerr<<"Usage: (in.jpg|in.avi) [cameraParams.yml] [markerSize] [outImage]"< Markers; - float MarkerSize=-1; - //read the input image + vector< Marker > Markers; + float MarkerSize = -1; + // read the input image cv::Mat InImage; - //try opening first as video + // try opening first as video VideoCapture vreader(argv[1]); if (vreader.isOpened()) { vreader.grab(); vreader.retrieve(InImage); + } else { + InImage = cv::imread(argv[1]); } - else { - InImage=cv::imread(argv[1]); - } - //at this point, we should have the image in InImage - //if empty, exit - if (InImage.total()==0) { - cerr<<"Could not open input"<=3) { + // read camera parameters if specifed + if (argc >= 3) { CamParam.readFromXMLFile(argv[2]); - //resizes the parameters to fit the size of the input image - CamParam.resize( InImage.size()); + // resizes the parameters to fit the size of the input image + CamParam.resize(InImage.size()); } - //read marker size if specified - if (argc>=4) MarkerSize=atof(argv[3]); - cv::namedWindow("in",1); + // read marker size if specified + if (argc >= 4) + MarkerSize = atof(argv[3]); + cv::namedWindow("in", 1); + - - //Ok, let's detect - MDetector.detect(InImage,Markers,CamParam,MarkerSize); - //for each marker, draw info and its boundaries in the image - for (unsigned int i=0;i=5) cv::imwrite(argv[4],InImage); + if (argc >= 5) + cv::imwrite(argv[4], InImage); } catch (std::exception &ex) { - cout<<"Exception :"< #include #include -#include -#include +#include +#include #include "aruco.h" #include "boarddetector.h" #include "cvdrawingutils.h" using namespace cv; -using namespace aruco; +using namespace aruco; /************************************ * * * * ************************************/ -int main(int argc,char **argv) -{ - try - { - if(argc<3) {cerr<<"Usage: image boardConfig.yml [cameraParams.yml] [markerSize] [outImage]"< Markers; - float MarkerSize=-1; - BoardConfiguration TheBoardConfig; - BoardDetector TheBoardDetector; - Board TheBoardDetected; - - cv::Mat InImage=cv::imread(argv[1]); - TheBoardConfig.readFromFile(argv[2]); - if (argc>=4) { - CamParam.readFromXMLFile(argv[3]); - //resizes the parameters to fit the size of the input image - CamParam.resize( InImage.size()); - } +int main(int argc, char **argv) { + try { + if (argc < 3) { + cerr << "Usage: image boardConfig.yml [cameraParams.yml] [markerSize] [outImage]" << endl; + exit(0); + } + aruco::CameraParameters CamParam; + MarkerDetector MDetector; + vector< Marker > Markers; + float MarkerSize = -1; + BoardConfiguration TheBoardConfig; + BoardDetector TheBoardDetector; + Board TheBoardDetected; - if (argc>=5) - MarkerSize=atof(argv[4]); - - cv::namedWindow("in",1); - MDetector.detect(InImage,Markers);//detect markers without computing R and T information - //Detection of the board - float probDetect=TheBoardDetector.detect( Markers, TheBoardConfig,TheBoardDetected, CamParam,MarkerSize); - - //for each marker, draw info and its boundaries in the image - for(unsigned int i=0;i= 4) { + CamParam.readFromXMLFile(argv[3]); + // resizes the parameters to fit the size of the input image + CamParam.resize(InImage.size()); + } - //draw a 3d cube in each marker if there is 3d info - if ( CamParam.isValid()){ - for(unsigned int i=0;i=6) cv::imwrite(argv[5],InImage); - - }catch(std::exception &ex) + if (argc >= 5) + MarkerSize = atof(argv[4]); - { - cout<<"Exception :"<= 6) + cv::imwrite(argv[5], InImage); + + } catch (std::exception &ex) + + { + cout << "Exception :" << ex.what() << endl; + } +} diff --git a/utils/aruco_test.cpp b/utils/aruco_test.cpp index ad8b276..81b2156 100644 --- a/utils/aruco_test.cpp +++ b/utils/aruco_test.cpp @@ -36,20 +36,20 @@ using namespace aruco; string TheInputVideo; string TheIntrinsicFile; -float TheMarkerSize=-1; +float TheMarkerSize = -1; int ThePyrDownLevel; MarkerDetector MDetector; VideoCapture TheVideoCapturer; -vector TheMarkers; -Mat TheInputImage,TheInputImageCopy; +vector< Marker > TheMarkers; +Mat TheInputImage, TheInputImageCopy; CameraParameters TheCameraParameters; -void cvTackBarEvents(int pos,void*); -bool readCameraParameters(string TheIntrinsicFile,CameraParameters &CP,Size size); +void cvTackBarEvents(int pos, void *); +bool readCameraParameters(string TheIntrinsicFile, CameraParameters &CP, Size size); -pair AvrgTime(0,0) ;//determines the average time required for detection -double ThresParam1,ThresParam2; -int iThresParam1,iThresParam2; -int waitTime=0; +pair< double, double > AvrgTime(0, 0); // determines the average time required for detection +double ThresParam1, ThresParam2; +int iThresParam1, iThresParam2; +int waitTime = 0; /************************************ * @@ -57,31 +57,29 @@ int waitTime=0; * * ************************************/ -bool readArguments ( int argc,char **argv ) -{ - if (argc<2) { - cerr<<"Invalid number of arguments"<=3) - TheIntrinsicFile=argv[2]; - if (argc>=4) - TheMarkerSize=atof(argv[3]); - - if (argc==3) - cerr<<"NOTE: You need makersize to see 3d info!!!!"<= 3) + TheIntrinsicFile = argv[2]; + if (argc >= 4) + TheMarkerSize = atof(argv[3]); + + if (argc == 3) + cerr << "NOTE: You need makersize to see 3d info!!!!" << endl; return true; } -int findParam ( std::string param,int argc, char *argv[] ) -{ - for ( int i=0; i>TheInputImage; - - //read camera parameters if passed - if (TheIntrinsicFile!="") { + bool isVideoFile = false; + if (TheInputVideo.find(".avi") != std::string::npos || TheInputVideo.find("live") != string::npos) + isVideoFile = true; + // read first image to get the dimensions + TheVideoCapturer >> TheInputImage; + + // read camera parameters if passed + if (TheIntrinsicFile != "") { TheCameraParameters.readFromXMLFile(TheIntrinsicFile); TheCameraParameters.resize(TheInputImage.size()); } - //Configure other parameters - if (ThePyrDownLevel>0) + // Configure other parameters + if (ThePyrDownLevel > 0) MDetector.pyrDown(ThePyrDownLevel); - //Create gui - - cv::namedWindow("thres",1); - cv::namedWindow("in",1); - MDetector.getThresholdParams( ThresParam1,ThresParam2); - MDetector.setCornerRefinementMethod(MarkerDetector::SUBPIX); - iThresParam1=ThresParam1; - iThresParam2=ThresParam2; - cv::createTrackbar("ThresParam1", "in",&iThresParam1, 13, cvTackBarEvents); - cv::createTrackbar("ThresParam2", "in",&iThresParam2, 13, cvTackBarEvents); - - char key=0; - int index=0; - //capture until press ESC or until the end of the video - do - { - TheVideoCapturer.retrieve( TheInputImage); - //copy image - - index++; //number of images captured - double tick = (double)getTickCount();//for checking the speed - //Detection of markers in the image passed - MDetector.detect(TheInputImage,TheMarkers,TheCameraParameters,TheMarkerSize); - //chekc the speed by calculating the mean speed of all iterations - AvrgTime.first+=((double)getTickCount()-tick)/getTickFrequency(); + // Create gui + + cv::namedWindow("thres", 1); + cv::namedWindow("in", 1); + + MDetector.setThresholdParams(7, 7); + MDetector.setThresholdParamRange(2, 0); + // MDetector.enableLockedCornersMethod(true); + // MDetector.setCornerRefinementMethod ( MarkerDetector::SUBPIX ); + MDetector.getThresholdParams(ThresParam1, ThresParam2); + iThresParam1 = ThresParam1; + iThresParam2 = ThresParam2; + cv::createTrackbar("ThresParam1", "in", &iThresParam1, 25, cvTackBarEvents); + cv::createTrackbar("ThresParam2", "in", &iThresParam2, 13, cvTackBarEvents); + + char key = 0; + int index = 0; + // capture until press ESC or until the end of the video + TheVideoCapturer.retrieve(TheInputImage); + + do { + + // copy image + + index++; // number of images captured + double tick = (double)getTickCount(); // for checking the speed + // Detection of markers in the image passed + MDetector.detect(TheInputImage, TheMarkers, TheCameraParameters, TheMarkerSize); + // chekc the speed by calculating the mean speed of all iterations + AvrgTime.first += ((double)getTickCount() - tick) / getTickFrequency(); AvrgTime.second++; - cout<<"\rTime detection="<<1000*AvrgTime.first/AvrgTime.second<<" milliseconds nmarkers="< AvrgTime(0,0) ;//determines the average time required for detection -double ThresParam1,ThresParam2; -int iThresParam1,iThresParam2; -int waitTime=0; +void cvTackBarEvents(int pos, void *); +pair< double, double > AvrgTime(0, 0); // determines the average time required for detection +double ThresParam1, ThresParam2; +int iThresParam1, iThresParam2; +int waitTime = 0; @@ -64,26 +64,25 @@ int waitTime=0; * ************************************/ -bool readArguments ( int argc,char **argv ) -{ +bool readArguments(int argc, char **argv) { - if (argc<3) { - cerr<<"Invalid number of arguments"<=4) - TheIntrinsicFile=argv[3]; - if (argc>=5) - TheMarkerSize=atof(argv[4]); - if (argc>=6) - TheOutVideoFilePath=argv[5]; + TheInputVideo = argv[1]; + TheBoardConfigFile = argv[2]; + if (argc >= 4) + TheIntrinsicFile = argv[3]; + if (argc >= 5) + TheMarkerSize = atof(argv[4]); + if (argc >= 6) + TheOutVideoFilePath = argv[5]; - if (argc==4) - cerr<<"NOTE: You need makersize to see 3d info!!!!"<>TheInputImage; + // read first image to get the dimensions + TheVideoCapturer >> TheInputImage; - //Open outputvideo - if ( TheOutVideoFilePath!="") - VWriter.open(TheOutVideoFilePath,CV_FOURCC('M','J','P','G'),15,TheInputImage.size()); + // Open outputvideo + if (TheOutVideoFilePath != "") + VWriter.open(TheOutVideoFilePath, CV_FOURCC('M', 'J', 'P', 'G'), 15, TheInputImage.size()); - //read camera parameters if passed - if (TheIntrinsicFile!="") { + // read camera parameters if passed + if (TheIntrinsicFile != "") { TheCameraParameters.readFromXMLFile(TheIntrinsicFile); TheCameraParameters.resize(TheInputImage.size()); } - //Create gui - - cv::namedWindow("thres",1); - cv::namedWindow("in",1); - TheBoardDetector.setParams(TheBoardConfig,TheCameraParameters,TheMarkerSize); - TheBoardDetector.getMarkerDetector().getThresholdParams( ThresParam1,ThresParam2); - TheBoardDetector.getMarkerDetector().setCornerRefinementMethod(MarkerDetector::HARRIS); - TheBoardDetector.set_repj_err_thres(1.5); -// TheBoardDetector.getMarkerDetector().enableErosion(true);//for chessboards - iThresParam1=ThresParam1; - iThresParam2=ThresParam2; - cv::createTrackbar("ThresParam1", "in",&iThresParam1, 13, cvTackBarEvents); - cv::createTrackbar("ThresParam2", "in",&iThresParam2, 13, cvTackBarEvents); - char key=0; - int index=0; - //capture until press ESC or until the end of the video - do - { - TheVideoCapturer.retrieve( TheInputImage); + // Create gui + + cv::namedWindow("thres", 1); + cv::namedWindow("in", 1); + TheBoardDetector.setParams(TheBoardConfig, TheCameraParameters, TheMarkerSize); + TheBoardDetector.getMarkerDetector().getThresholdParams(ThresParam1, ThresParam2); + TheBoardDetector.getMarkerDetector().setCornerRefinementMethod(MarkerDetector::HARRIS); + TheBoardDetector.set_repj_err_thres(1.5); + // TheBoardDetector.getMarkerDetector().enableErosion(true);//for chessboards + iThresParam1 = ThresParam1; + iThresParam2 = ThresParam2; + cv::createTrackbar("ThresParam1", "in", &iThresParam1, 13, cvTackBarEvents); + cv::createTrackbar("ThresParam2", "in", &iThresParam2, 13, cvTackBarEvents); + char key = 0; + int index = 0; + // capture until press ESC or until the end of the video + do { + TheVideoCapturer.retrieve(TheInputImage); TheInputImage.copyTo(TheInputImageCopy); - index++; //number of images captured - double tick = (double)getTickCount();//for checking the speed - //Detection of the board - float probDetect=TheBoardDetector.detect(TheInputImage); - //chekc the speed by calculating the mean speed of all iterations - AvrgTime.first+=((double)getTickCount()-tick)/getTickFrequency(); + index++; // number of images captured + double tick = (double)getTickCount(); // for checking the speed + // Detection of the board + float probDetect = TheBoardDetector.detect(TheInputImage); + // chekc the speed by calculating the mean speed of all iterations + AvrgTime.first += ((double)getTickCount() - tick) / getTickFrequency(); AvrgTime.second++; - cout<<"Time detection="<<1000*AvrgTime.first/AvrgTime.second<<" milliseconds"<0.2) { - CvDrawingUtils::draw3dAxis( TheInputImageCopy,TheBoardDetector.getDetectedBoard(),TheCameraParameters); -// CvDrawingUtils::draw3dCube(TheInputImageCopy, TheBoardDetector.getDetectedBoard(),TheCameraParameters); - //draw3dBoardCube( TheInputImageCopy,TheBoardDetected,TheIntriscCameraMatrix,TheDistorsionCameraParams); + cout << "Time detection=" << 1000 * AvrgTime.first / AvrgTime.second << " milliseconds" << endl; + // print marker borders + for (unsigned int i = 0; i < TheBoardDetector.getDetectedMarkers().size(); i++) + TheBoardDetector.getDetectedMarkers()[i].draw(TheInputImageCopy, Scalar(0, 0, 255), 1); + + // print board + if (TheCameraParameters.isValid()) { + if (probDetect > 0.2) { + CvDrawingUtils::draw3dAxis(TheInputImageCopy, TheBoardDetector.getDetectedBoard(), TheCameraParameters); + // CvDrawingUtils::draw3dCube(TheInputImageCopy, TheBoardDetector.getDetectedBoard(),TheCameraParameters); + // draw3dBoardCube( TheInputImageCopy,TheBoardDetected,TheIntriscCameraMatrix,TheDistorsionCameraParams); } } - //DONE! Easy, right? - - //show input with augmented information and the thresholded image - cv::imshow("in",TheInputImageCopy); - cv::imshow("thres",TheBoardDetector.getMarkerDetector().getThresholdedImage()); - //write to video if required - if ( TheOutVideoFilePath!="") { - //create a beautiful compiosed image showing the thresholded - //first create a small version of the thresholded image + // DONE! Easy, right? + + // show input with augmented information and the thresholded image + cv::imshow("in", TheInputImageCopy); + cv::imshow("thres", TheBoardDetector.getMarkerDetector().getThresholdedImage()); + // write to video if required + if (TheOutVideoFilePath != "") { + // create a beautiful compiosed image showing the thresholded + // first create a small version of the thresholded image cv::Mat smallThres; - cv::resize( TheBoardDetector.getMarkerDetector().getThresholdedImage(),smallThres,cvSize(TheInputImageCopy.cols/3,TheInputImageCopy.rows/3)); + cv::resize(TheBoardDetector.getMarkerDetector().getThresholdedImage(), smallThres, + cvSize(TheInputImageCopy.cols / 3, TheInputImageCopy.rows / 3)); cv::Mat small3C; - cv::cvtColor(smallThres,small3C,CV_GRAY2BGR); - cv::Mat roi=TheInputImageCopy(cv::Rect(0,0,TheInputImageCopy.cols/3,TheInputImageCopy.rows/3)); + cv::cvtColor(smallThres, small3C, CV_GRAY2BGR); + cv::Mat roi = TheInputImageCopy(cv::Rect(0, 0, TheInputImageCopy.cols / 3, TheInputImageCopy.rows / 3)); small3C.copyTo(roi); - VWriter<0.2) - aruco::CvDrawingUtils::draw3dAxis(TheInputImageCopy,TheBoardDetector.getDetectedBoard(),TheCameraParameters); - - - cv::imshow("in",TheInputImageCopy); - cv::imshow("thres",TheBoardDetector.getMarkerDetector().getThresholdedImage()); -} - + if (TheCameraParameters.isValid() && probDetect > 0.2) + aruco::CvDrawingUtils::draw3dAxis(TheInputImageCopy, TheBoardDetector.getDetectedBoard(), TheCameraParameters); + cv::imshow("in", TheInputImageCopy); + cv::imshow("thres", TheBoardDetector.getMarkerDetector().getThresholdedImage()); +} diff --git a/utils/aruco_test_board_gl.cpp b/utils/aruco_test_board_gl.cpp index 68e9897..0ff838a 100644 --- a/utils/aruco_test_board_gl.cpp +++ b/utils/aruco_test_board_gl.cpp @@ -34,7 +34,7 @@ or implied, of Rafael Muñoz Salinas. //http://social.msdn.microsoft.com/Forums/eu/vcgeneral/thread/7d6e6fa5-afc2-4370-9a1f-991a76ccb5b7 #include #include - #include + #include #else #include #include diff --git a/utils/aruco_test_board_gl_mask.cpp b/utils/aruco_test_board_gl_mask.cpp new file mode 100644 index 0000000..3b827d2 --- /dev/null +++ b/utils/aruco_test_board_gl_mask.cpp @@ -0,0 +1,372 @@ +/***************************** +Copyright 2011 Rafael Muñoz Salinas. All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are +permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this list of + conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, this list + of conditions and the following disclaimer in the documentation and/or other materials + provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND +FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR +CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +The views and conclusions contained in the software and documentation are those of the +authors and should not be interpreted as representing official policies, either expressed +or implied, of Rafael Muñoz Salinas. +********************************/ +#include +#include +#include +#ifdef __APPLE__ + #include +#elif _MSC_VER + //http://social.msdn.microsoft.com/Forums/eu/vcgeneral/thread/7d6e6fa5-afc2-4370-9a1f-991a76ccb5b7 + #include + #include + #include +#else + #include + #include +#endif + +#include +#include "aruco.h" +#include "boarddetector.h" +#include "common.h" + + +using namespace cv; +using namespace aruco; + +string TheInputVideo,TheIntrinsicFile,TheBoardConfigFile; +bool The3DInfoAvailable=false; +float TheMarkerSize=-1; +MarkerDetector MDetector; +VideoCapture TheVideoCapturer; +vector TheMarkers; +//board +BoardDetector TheBoardDetector; +pair TheBoardDetected; //the board and its probabilit +BoardConfiguration TheBoardConfig; +Mat TheInputImage,TheUndInputImage,TheResizedImage,TheMask; +CameraParameters TheCameraParams; +Size TheGlWindowSize; +bool TheCaptureFlag=true; +void vDrawScene(); +void vIdle(); +void vResize( GLsizei iWidth, GLsizei iHeight ); +void vMouse(int b,int s,int x,int y); + + + +/************************************ + * + * + * + * + ************************************/ +bool readArguments ( int argc,char **argv ) +{ + + if (argc!=5) { + cerr<<"Invalid number of arguments"<>TheInputImage; + //read camera paramters if passed + TheCameraParams.readFromXMLFile(TheIntrinsicFile); + TheCameraParams.resize( TheInputImage.size()); + + TheBoardDetector.getMarkerDetector().setThresholdParams(25,7); + + glutInit(&argc, argv); + glutInitWindowPosition( 0, 0); + glutInitWindowSize(TheInputImage.size().width,TheInputImage.size().height); + glutInitDisplayMode( GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE ); + glutCreateWindow( "ArUco" ); + glutDisplayFunc( vDrawScene ); + glutIdleFunc( vIdle ); + glutReshapeFunc( vResize ); + glutMouseFunc(vMouse); + glClearColor( 0.0, 0.0, 0.0, 1.0 ); + glClearDepth( 1.0 ); + + // these two are necesary for the mask effect + glEnable( GL_ALPHA_TEST ); + glAlphaFunc( GL_GREATER, 0.5 ); + + TheGlWindowSize=TheInputImage.size(); + vResize(TheGlWindowSize.width,TheGlWindowSize.height); + glutMainLoop(); + + } catch (std::exception &ex) + + { + cout<<"Exception :"<