Commit fb0338fb authored by Vladislav Sovrasov's avatar Vladislav Sovrasov

Merge branch 'master' into text_detector_dnn

parents 3253fe9f 374e3218
...@@ -26,14 +26,14 @@ corners) are employed. ...@@ -26,14 +26,14 @@ corners) are employed.
The aruco module allows the use of Boards. The main class is the ```cv::aruco::Board``` class which defines the Board layout: The aruco module allows the use of Boards. The main class is the ```cv::aruco::Board``` class which defines the Board layout:
``` c++ @code{.cpp}
class Board { class Board {
public: public:
std::vector<std::vector<cv::Point3f> > objPoints; std::vector<std::vector<cv::Point3f> > objPoints;
cv::Ptr<cv::aruco::Dictionary> dictionary; cv::Ptr<cv::aruco::Dictionary> dictionary;
std::vector<int> ids; std::vector<int> ids;
}; };
``` @endcode
A object of type ```Board``` has three parameters: A object of type ```Board``` has three parameters:
- The ```objPoints``` structure is the list of corner positions in the 3d Board reference system, i.e. its layout. - The ```objPoints``` structure is the list of corner positions in the 3d Board reference system, i.e. its layout.
...@@ -51,7 +51,7 @@ In fact, to use marker boards, a standard marker detection should be done before ...@@ -51,7 +51,7 @@ In fact, to use marker boards, a standard marker detection should be done before
The aruco module provides a specific function, ```estimatePoseBoard()```, to perform pose estimation for boards: The aruco module provides a specific function, ```estimatePoseBoard()```, to perform pose estimation for boards:
``` c++ @code{.cpp}
cv::Mat inputImage; cv::Mat inputImage;
// camera parameters are read from somewhere // camera parameters are read from somewhere
cv::Mat cameraMatrix, distCoeffs; cv::Mat cameraMatrix, distCoeffs;
...@@ -67,7 +67,7 @@ The aruco module provides a specific function, ```estimatePoseBoard()```, to per ...@@ -67,7 +67,7 @@ The aruco module provides a specific function, ```estimatePoseBoard()```, to per
cv::Vec3d rvec, tvec; cv::Vec3d rvec, tvec;
int valid = cv::aruco::estimatePoseBoard(markerCorners, markerIds, board, cameraMatrix, distCoeffs, rvec, tvec); int valid = cv::aruco::estimatePoseBoard(markerCorners, markerIds, board, cameraMatrix, distCoeffs, rvec, tvec);
} }
``` @endcode
The parameters of estimatePoseBoard are: The parameters of estimatePoseBoard are:
...@@ -120,9 +120,9 @@ A ```GridBoard``` object can be defined using the following parameters: ...@@ -120,9 +120,9 @@ A ```GridBoard``` object can be defined using the following parameters:
This object can be easily created from these parameters using the ```cv::aruco::GridBoard::create()``` static function: This object can be easily created from these parameters using the ```cv::aruco::GridBoard::create()``` static function:
``` c++ @code{.cpp}
cv::aruco::GridBoard board = cv::aruco::GridBoard::create(5, 7, 0.04, 0.01, dictionary); cv::aruco::GridBoard board = cv::aruco::GridBoard::create(5, 7, 0.04, 0.01, dictionary);
``` @endcode
- The first and second parameters are the number of markers in the X and Y direction respectively. - The first and second parameters are the number of markers in the X and Y direction respectively.
- The third and fourth parameters are the marker length and the marker separation respectively. They can be provided - The third and fourth parameters are the marker length and the marker separation respectively. They can be provided
...@@ -136,11 +136,11 @@ through ```board.ids```, like in the ```Board``` parent class. ...@@ -136,11 +136,11 @@ through ```board.ids```, like in the ```Board``` parent class.
After creating a Grid Board, we probably want to print it and use it. A function to generate the image After creating a Grid Board, we probably want to print it and use it. A function to generate the image
of a ```GridBoard``` is provided in ```cv::aruco::GridBoard::draw()```. For example: of a ```GridBoard``` is provided in ```cv::aruco::GridBoard::draw()```. For example:
``` c++ @code{.cpp}
cv::Ptr<cv::aruco::GridBoard> board = cv::aruco::GridBoard::create(5, 7, 0.04, 0.01, dictionary); cv::Ptr<cv::aruco::GridBoard> board = cv::aruco::GridBoard::create(5, 7, 0.04, 0.01, dictionary);
cv::Mat boardImage; cv::Mat boardImage;
board->draw( cv::Size(600, 500), boardImage, 10, 1 ); board->draw( cv::Size(600, 500), boardImage, 10, 1 );
``` @endcode
- The first parameter is the size of the output image in pixels. In this case 600x500 pixels. If this is not proportional - The first parameter is the size of the output image in pixels. In this case 600x500 pixels. If this is not proportional
to the board dimensions, it will be centered on the image. to the board dimensions, it will be centered on the image.
...@@ -156,13 +156,13 @@ The output image will be something like this: ...@@ -156,13 +156,13 @@ The output image will be something like this:
A full working example of board creation is included in the ```create_board.cpp``` inside the module samples folder. A full working example of board creation is included in the ```create_board.cpp``` inside the module samples folder.
Note: The samples now take input via commandline via the [OpenCV Commandline Parser](http://docs.opencv.org/trunk/d0/d2e/classcv_1_1CommandLineParser.html#gsc.tab=0). For this file the example parameters will look like Note: The samples now take input via commandline via the [OpenCV Commandline Parser](http://docs.opencv.org/trunk/d0/d2e/classcv_1_1CommandLineParser.html#gsc.tab=0). For this file the example parameters will look like
``` c++ @code{.cpp}
"_output path_/aboard.png" -w=5 -h=7 -l=100 -s=10 -d=10 "_output path_/aboard.png" -w=5 -h=7 -l=100 -s=10 -d=10
``` @endcode
Finally, a full example of board detection: Finally, a full example of board detection:
``` c++ @code{.cpp}
cv::VideoCapture inputVideo; cv::VideoCapture inputVideo;
inputVideo.open(0); inputVideo.open(0);
...@@ -199,7 +199,7 @@ Finally, a full example of board detection: ...@@ -199,7 +199,7 @@ Finally, a full example of board detection:
if (key == 27) if (key == 27)
break; break;
} }
``` @endcode
Sample video: Sample video:
...@@ -210,9 +210,9 @@ Sample video: ...@@ -210,9 +210,9 @@ Sample video:
A full working example is included in the ```detect_board.cpp``` inside the module samples folder. A full working example is included in the ```detect_board.cpp``` inside the module samples folder.
Note: The samples now take input via commandline via the [OpenCV Commandline Parser](http://docs.opencv.org/trunk/d0/d2e/classcv_1_1CommandLineParser.html#gsc.tab=0). For this file the example parameters will look like Note: The samples now take input via commandline via the [OpenCV Commandline Parser](http://docs.opencv.org/trunk/d0/d2e/classcv_1_1CommandLineParser.html#gsc.tab=0). For this file the example parameters will look like
``` c++ @code{.cpp}
-c="_path_"/calib.txt" "_path_/aboard.png" -w=5 -h=7 -l=100 -s=10 -d=10 -c="_path_"/calib.txt" "_path_/aboard.png" -w=5 -h=7 -l=100 -s=10 -d=10
``` @endcode
...@@ -255,7 +255,7 @@ internal bits are not analyzed at all and only the corner distances are evaluate ...@@ -255,7 +255,7 @@ internal bits are not analyzed at all and only the corner distances are evaluate
This is an example of using the ```refineDetectedMarkers()``` function: This is an example of using the ```refineDetectedMarkers()``` function:
``` c++ @code{.cpp}
cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250); cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);
cv::Ptr<cv::aruco::GridBoard> board = cv::aruco::GridBoard::create(5, 7, 0.04, 0.01, dictionary); cv::Ptr<cv::aruco::GridBoard> board = cv::aruco::GridBoard::create(5, 7, 0.04, 0.01, dictionary);
std::vector<int> markerIds; std::vector<int> markerIds;
...@@ -265,7 +265,7 @@ This is an example of using the ```refineDetectedMarkers()``` function: ...@@ -265,7 +265,7 @@ This is an example of using the ```refineDetectedMarkers()``` function:
cv::aruco::refineDetectedMarkersinputImage, board, markerCorners, markerIds, rejectedCandidates); cv::aruco::refineDetectedMarkersinputImage, board, markerCorners, markerIds, rejectedCandidates);
// After calling this function, if any new marker has been detected it will be removed from rejectedCandidates and included // After calling this function, if any new marker has been detected it will be removed from rejectedCandidates and included
// at the end of markerCorners and markerIds // at the end of markerCorners and markerIds
``` @endcode
It must also be noted that, in some cases, if the number of detected markers in the first place is too low (for instance only It must also be noted that, in some cases, if the number of detected markers in the first place is too low (for instance only
1 or 2 markers), the projections of the missing markers can be of bad quality, producing erroneous correspondences. 1 or 2 markers), the projections of the missing markers can be of bad quality, producing erroneous correspondences.
......
...@@ -32,7 +32,7 @@ visible in all the viewpoints. ...@@ -32,7 +32,7 @@ visible in all the viewpoints.
The function to calibrate is ```calibrateCameraCharuco()```. Example: The function to calibrate is ```calibrateCameraCharuco()```. Example:
``` c++ @code{.cpp}
cv::Ptr<aruco::CharucoBoard> board = ... // create charuco board cv::Ptr<aruco::CharucoBoard> board = ... // create charuco board
cv::Size imgSize = ... // camera image size cv::Size imgSize = ... // camera image size
...@@ -48,7 +48,7 @@ The function to calibrate is ```calibrateCameraCharuco()```. Example: ...@@ -48,7 +48,7 @@ The function to calibrate is ```calibrateCameraCharuco()```. Example:
int calibrationFlags = ... // Set calibration flags (same than in calibrateCamera() function) int calibrationFlags = ... // Set calibration flags (same than in calibrateCamera() function)
double repError = cv::aruco::calibrateCameraCharuco(allCharucoCorners, allCharucoIds, board, imgSize, cameraMatrix, distCoeffs, rvecs, tvecs, calibrationFlags); double repError = cv::aruco::calibrateCameraCharuco(allCharucoCorners, allCharucoIds, board, imgSize, cameraMatrix, distCoeffs, rvecs, tvecs, calibrationFlags);
``` @endcode
The ChArUco corners and ChArUco identifiers captured on each viewpoint are stored in the vectors ```allCharucoCorners``` and ```allCharucoIds```, one element per viewpoint. The ChArUco corners and ChArUco identifiers captured on each viewpoint are stored in the vectors ```allCharucoCorners``` and ```allCharucoIds```, one element per viewpoint.
...@@ -62,9 +62,9 @@ Finally, the ```calibrationFlags``` parameter determines some of the options for ...@@ -62,9 +62,9 @@ Finally, the ```calibrationFlags``` parameter determines some of the options for
A full working example is included in the ```calibrate_camera_charuco.cpp``` inside the module samples folder. A full working example is included in the ```calibrate_camera_charuco.cpp``` inside the module samples folder.
Note: The samples now take input via commandline via the [OpenCV Commandline Parser](http://docs.opencv.org/trunk/d0/d2e/classcv_1_1CommandLineParser.html#gsc.tab=0). For this file the example parameters will look like Note: The samples now take input via commandline via the [OpenCV Commandline Parser](http://docs.opencv.org/trunk/d0/d2e/classcv_1_1CommandLineParser.html#gsc.tab=0). For this file the example parameters will look like
``` c++ @code{.cpp}
_output path_" -dp="_path_/detector_params.yml" -w=5 -h=7 -sl=0.04 -ml=0.02 -d=10 _output path_" -dp="_path_/detector_params.yml" -w=5 -h=7 -sl=0.04 -ml=0.02 -d=10
``` @endcode
...@@ -80,7 +80,7 @@ requires the detections of an ArUco board from different viewpoints. ...@@ -80,7 +80,7 @@ requires the detections of an ArUco board from different viewpoints.
Example of ```calibrateCameraAruco()``` use: Example of ```calibrateCameraAruco()``` use:
``` c++ @code{.cpp}
cv::Ptr<aruco::Board> board = ... // create aruco board cv::Ptr<aruco::Board> board = ... // create aruco board
cv::Size imgSize = ... // camera image size cv::Size imgSize = ... // camera image size
...@@ -97,7 +97,7 @@ Example of ```calibrateCameraAruco()``` use: ...@@ -97,7 +97,7 @@ Example of ```calibrateCameraAruco()``` use:
int calibrationFlags = ... // Set calibration flags (same than in calibrateCamera() function) int calibrationFlags = ... // Set calibration flags (same than in calibrateCamera() function)
double repError = cv::aruco::calibrateCameraAruco(allCornersConcatenated, allIdsConcatenated, markerCounterPerFrame, board, imgSize, cameraMatrix, distCoeffs, rvecs, tvecs, calibrationFlags); double repError = cv::aruco::calibrateCameraAruco(allCornersConcatenated, allIdsConcatenated, markerCounterPerFrame, board, imgSize, cameraMatrix, distCoeffs, rvecs, tvecs, calibrationFlags);
``` @endcode
In this case, and contrary to the ```calibrateCameraCharuco()``` function, the detected markers on each viewpoint are concatenated in the arrays ```allCornersConcatenated``` and In this case, and contrary to the ```calibrateCameraCharuco()``` function, the detected markers on each viewpoint are concatenated in the arrays ```allCornersConcatenated``` and
```allCornersConcatenated``` (the first two parameters). The third parameter, the array ```markerCounterPerFrame```, indicates the number of marker detected on each viewpoint. ```allCornersConcatenated``` (the first two parameters). The third parameter, the array ```markerCounterPerFrame```, indicates the number of marker detected on each viewpoint.
...@@ -107,6 +107,6 @@ any ```Board``` object. ...@@ -107,6 +107,6 @@ any ```Board``` object.
A full working example is included in the ```calibrate_camera.cpp``` inside the module samples folder. A full working example is included in the ```calibrate_camera.cpp``` inside the module samples folder.
Note: The samples now take input via commandline via the [OpenCV Commandline Parser](http://docs.opencv.org/trunk/d0/d2e/classcv_1_1CommandLineParser.html#gsc.tab=0). For this file the example parameters will look like Note: The samples now take input via commandline via the [OpenCV Commandline Parser](http://docs.opencv.org/trunk/d0/d2e/classcv_1_1CommandLineParser.html#gsc.tab=0). For this file the example parameters will look like
``` c++ @code{.cpp}
"_path_/calib.txt" -w=5 -h=7 -l=100 -s=10 -d=10 "_path_/calib.txt" -w=5 -h=7 -l=100 -s=10 -d=10
``` @endcode
...@@ -19,9 +19,9 @@ a popular library for detection of square fiducial markers developed by Rafael M ...@@ -19,9 +19,9 @@ a popular library for detection of square fiducial markers developed by Rafael M
> Pattern Recogn. 47, 6 (June 2014), 2280-2292. DOI=10.1016/j.patcog.2014.01.005 > Pattern Recogn. 47, 6 (June 2014), 2280-2292. DOI=10.1016/j.patcog.2014.01.005
The aruco functionalities are included in: The aruco functionalities are included in:
``` c++ @code{.cpp}
#include <opencv2/aruco.hpp> #include <opencv2/aruco.hpp>
``` @endcode
Markers and Dictionaries Markers and Dictionaries
...@@ -69,11 +69,11 @@ Marker images can be generated using the ```drawMarker()``` function. ...@@ -69,11 +69,11 @@ Marker images can be generated using the ```drawMarker()``` function.
For example, lets analyze the following call: For example, lets analyze the following call:
``` c++ @code{.cpp}
cv::Mat markerImage; cv::Mat markerImage;
cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250); cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);
cv::aruco::drawMarker(dictionary, 23, 200, markerImage, 1); cv::aruco::drawMarker(dictionary, 23, 200, markerImage, 1);
``` @endcode
First, the ```Dictionary``` object is created by choosing one of the predefined dictionaries in the aruco module. First, the ```Dictionary``` object is created by choosing one of the predefined dictionaries in the aruco module.
Concretely, this dictionary is composed by 250 markers and a marker size of 6x6 bits (```DICT_6X6_250```). Concretely, this dictionary is composed by 250 markers and a marker size of 6x6 bits (```DICT_6X6_250```).
...@@ -104,9 +104,9 @@ The generated image is: ...@@ -104,9 +104,9 @@ The generated image is:
A full working example is included in the ```create_marker.cpp``` inside the module samples folder. A full working example is included in the ```create_marker.cpp``` inside the module samples folder.
Note: The samples now take input via commandline via the [OpenCV Commandline Parser](http://docs.opencv.org/trunk/d0/d2e/classcv_1_1CommandLineParser.html#gsc.tab=0). For this file the example parameters will look like Note: The samples now take input via commandline via the [OpenCV Commandline Parser](http://docs.opencv.org/trunk/d0/d2e/classcv_1_1CommandLineParser.html#gsc.tab=0). For this file the example parameters will look like
``` c++ @code{.cpp}
"/Users/Sarthak/Dropbox/OpenCV_GSoC/marker.png" -d=10 -id=1 "/Users/Sarthak/Dropbox/OpenCV_GSoC/marker.png" -d=10 -id=1
``` @endcode
Marker Detection Marker Detection
------ ------
...@@ -153,7 +153,7 @@ previous detected markers returned by ```detectMarkers()```. ...@@ -153,7 +153,7 @@ previous detected markers returned by ```detectMarkers()```.
An example of marker detection: An example of marker detection:
``` c++ @code{.cpp}
cv::Mat inputImage; cv::Mat inputImage;
... ...
std::vector<int> markerIds; std::vector<int> markerIds;
...@@ -161,7 +161,7 @@ An example of marker detection: ...@@ -161,7 +161,7 @@ An example of marker detection:
cv::Ptr<cv::aruco::DetectorParameters> parameters; cv::Ptr<cv::aruco::DetectorParameters> parameters;
cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250); cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);
cv::aruco::detectMarkers(inputImage, dictionary, markerCorners, markerIds, parameters, rejectedCandidates); cv::aruco::detectMarkers(inputImage, dictionary, markerCorners, markerIds, parameters, rejectedCandidates);
``` @endcode
The parameters of ```detectMarkers``` are: The parameters of ```detectMarkers``` are:
...@@ -185,10 +185,10 @@ The next thing you probably want to do after ```detectMarkers()``` is checking t ...@@ -185,10 +185,10 @@ The next thing you probably want to do after ```detectMarkers()``` is checking t
been correctly detected. Fortunately, the aruco module provides a function to draw the detected been correctly detected. Fortunately, the aruco module provides a function to draw the detected
markers in the input image, this function is ```drawDetectedMarkers()```. For example: markers in the input image, this function is ```drawDetectedMarkers()```. For example:
``` c++ @code{.cpp}
cv::Mat outputImage cv::Mat outputImage
cv::aruco::drawDetectedMarkers(image, markerCorners, markerIds); cv::aruco::drawDetectedMarkers(image, markerCorners, markerIds);
``` @endcode
- ```image``` is the input/output image where the markers will be drawn (it will normally be the same image where the markers were detected). - ```image``` is the input/output image where the markers will be drawn (it will normally be the same image where the markers were detected).
- ```markerCorners``` and ```markerIds``` are the structures of the detected markers in the same format - ```markerCorners``` and ```markerIds``` are the structures of the detected markers in the same format
...@@ -201,7 +201,7 @@ Note that this function is only provided for visualization and its use can be pe ...@@ -201,7 +201,7 @@ Note that this function is only provided for visualization and its use can be pe
With these two functions we can create a basic marker detection loop to detect markers from our With these two functions we can create a basic marker detection loop to detect markers from our
camera: camera:
``` c++ @code{.cpp}
cv::VideoCapture inputVideo; cv::VideoCapture inputVideo;
inputVideo.open(0); inputVideo.open(0);
cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250); cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);
...@@ -223,7 +223,7 @@ camera: ...@@ -223,7 +223,7 @@ camera:
if (key == 27) if (key == 27)
break; break;
} }
``` @endcode
Note that some of the optional parameters have been omitted, like the detection parameter object or the Note that some of the optional parameters have been omitted, like the detection parameter object or the
output vector of rejected candidates. output vector of rejected candidates.
...@@ -231,9 +231,9 @@ output vector of rejected candidates. ...@@ -231,9 +231,9 @@ output vector of rejected candidates.
A full working example is included in the ```detect_markers.cpp``` inside the module samples folder. A full working example is included in the ```detect_markers.cpp``` inside the module samples folder.
Note: The samples now take input via commandline via the [OpenCV Commandline Parser](http://docs.opencv.org/trunk/d0/d2e/classcv_1_1CommandLineParser.html#gsc.tab=0). For this file the example parameters will look like Note: The samples now take input via commandline via the [OpenCV Commandline Parser](http://docs.opencv.org/trunk/d0/d2e/classcv_1_1CommandLineParser.html#gsc.tab=0). For this file the example parameters will look like
``` c++ @code{.cpp}
-c="_path_/calib.txt" -d=10 -c="_path_/calib.txt" -d=10
``` @endcode
...@@ -262,12 +262,12 @@ information). ...@@ -262,12 +262,12 @@ information).
The aruco module provides a function to estimate the poses of all the detected markers: The aruco module provides a function to estimate the poses of all the detected markers:
``` c++ @code{.cpp}
cv::Mat cameraMatrix, distCoeffs; cv::Mat cameraMatrix, distCoeffs;
... ...
std::vector<cv::Vec3d> rvecs, tvecs; std::vector<cv::Vec3d> rvecs, tvecs;
cv::aruco::estimatePoseSingleMarkers(corners, 0.05, cameraMatrix, distCoeffs, rvecs, tvecs); cv::aruco::estimatePoseSingleMarkers(corners, 0.05, cameraMatrix, distCoeffs, rvecs, tvecs);
``` @endcode
- The ```corners``` parameter is the vector of marker corners returned by the ```detectMarkers()``` function. - The ```corners``` parameter is the vector of marker corners returned by the ```detectMarkers()``` function.
- The second parameter is the size of the marker side in meters or in any other unit. Note that the - The second parameter is the size of the marker side in meters or in any other unit. Note that the
...@@ -284,9 +284,9 @@ with the Z axis pointing out, as in the following image. Axis-color corresponden ...@@ -284,9 +284,9 @@ with the Z axis pointing out, as in the following image. Axis-color corresponden
The aruco module provides a function to draw the axis as in the image above, so pose estimation can be The aruco module provides a function to draw the axis as in the image above, so pose estimation can be
checked: checked:
``` c++ @code{.cpp}
cv::aruco::drawAxis(image, cameraMatrix, distCoeffs, rvec, tvec, 0.1); cv::aruco::drawAxis(image, cameraMatrix, distCoeffs, rvec, tvec, 0.1);
``` @endcode
- ```image``` is the input/output image where the axis will be drawn (it will normally be the same image where the markers were detected). - ```image``` is the input/output image where the axis will be drawn (it will normally be the same image where the markers were detected).
- ```cameraMatrix``` and ```distCoeffs``` are the camera calibration parameters. - ```cameraMatrix``` and ```distCoeffs``` are the camera calibration parameters.
...@@ -295,7 +295,7 @@ checked: ...@@ -295,7 +295,7 @@ checked:
A basic full example for pose estimation from single markers: A basic full example for pose estimation from single markers:
``` c++ @code{.cpp}
cv::VideoCapture inputVideo; cv::VideoCapture inputVideo;
inputVideo.open(0); inputVideo.open(0);
...@@ -330,7 +330,7 @@ A basic full example for pose estimation from single markers: ...@@ -330,7 +330,7 @@ A basic full example for pose estimation from single markers:
if (key == 27) if (key == 27)
break; break;
} }
``` @endcode
Sample video: Sample video:
...@@ -341,9 +341,9 @@ Sample video: ...@@ -341,9 +341,9 @@ Sample video:
A full working example is included in the ```detect_markers.cpp``` inside the module samples folder. A full working example is included in the ```detect_markers.cpp``` inside the module samples folder.
Note: The samples now take input via commandline via the [OpenCV Commandline Parser](http://docs.opencv.org/trunk/d0/d2e/classcv_1_1CommandLineParser.html#gsc.tab=0). For this file the example parameters will look like Note: The samples now take input via commandline via the [OpenCV Commandline Parser](http://docs.opencv.org/trunk/d0/d2e/classcv_1_1CommandLineParser.html#gsc.tab=0). For this file the example parameters will look like
``` c++ @code{.cpp}
-c="_path_/calib.txt" -d=10 -c="_path_/calib.txt" -d=10
``` @endcode
...@@ -373,9 +373,9 @@ you can increase your system robustness: ...@@ -373,9 +373,9 @@ you can increase your system robustness:
This is the easiest way to select a dictionary. The aruco module includes a set of predefined dictionaries This is the easiest way to select a dictionary. The aruco module includes a set of predefined dictionaries
of a variety of marker sizes and number of markers. For instance: of a variety of marker sizes and number of markers. For instance:
``` c++ @code{.cpp}
cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250); cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);
``` @endcode
DICT_6X6_250 is an example of predefined dictionary of markers with 6x6 bits and a total of 250 DICT_6X6_250 is an example of predefined dictionary of markers with 6x6 bits and a total of 250
markers. markers.
...@@ -389,9 +389,9 @@ The smaller the dictionary, the higher the inter-marker distance. ...@@ -389,9 +389,9 @@ The smaller the dictionary, the higher the inter-marker distance.
The dictionary can be generated automatically to adjust to the desired number of markers and bits, so that The dictionary can be generated automatically to adjust to the desired number of markers and bits, so that
the inter-marker distance is optimized: the inter-marker distance is optimized:
``` c++ @code{.cpp}
cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::generateCustomDictionary(36, 5); cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::generateCustomDictionary(36, 5);
``` @endcode
This will generate a customized dictionary composed by 36 markers of 5x5 bits. The process can take several This will generate a customized dictionary composed by 36 markers of 5x5 bits. The process can take several
seconds, depending on the parameters (it is slower for larger dictionaries and higher number of bits). seconds, depending on the parameters (it is slower for larger dictionaries and higher number of bits).
...@@ -405,7 +405,7 @@ the ```Dictionary``` object parameters need to be assigned manually. It must be ...@@ -405,7 +405,7 @@ the ```Dictionary``` object parameters need to be assigned manually. It must be
The ```Dictionary``` parameters are: The ```Dictionary``` parameters are:
``` c++ @code{.cpp}
class Dictionary { class Dictionary {
public: public:
...@@ -416,10 +416,9 @@ The ```Dictionary``` parameters are: ...@@ -416,10 +416,9 @@ The ```Dictionary``` parameters are:
... ...
} }
@endcode
``` <code>bytesList</code> is the array that contains all the information about the marker codes. ```markerSize``` is the size
```bytesList``` is the array that contains all the information about the marker codes. ```markerSize``` is the size
of each marker dimension (for instance, 5 for markers with 5x5 bits). Finally, ```maxCorrectionBits``` is of each marker dimension (for instance, 5 for markers with 5x5 bits). Finally, ```maxCorrectionBits``` is
the maximum number of erroneous bits that can be corrected during the marker detection. If this value is too the maximum number of erroneous bits that can be corrected during the marker detection. If this value is too
high, it can lead to a high amount of false positives. high, it can lead to a high amount of false positives.
...@@ -431,7 +430,7 @@ Fortunately, a marker can be easily transformed to this form using the static me ...@@ -431,7 +430,7 @@ Fortunately, a marker can be easily transformed to this form using the static me
For example: For example:
``` c++ @code{.cpp}
cv::aruco::Dictionary dictionary; cv::aruco::Dictionary dictionary;
// markers of 6x6 bits // markers of 6x6 bits
dictionary.markerSize = 6; dictionary.markerSize = 6;
...@@ -448,8 +447,7 @@ For example: ...@@ -448,8 +447,7 @@ For example:
// add the marker as a new row // add the marker as a new row
dictionary.bytesList.push_back(markerCompressed); dictionary.bytesList.push_back(markerCompressed);
} }
@endcode
```
......
...@@ -28,9 +28,9 @@ The aruco module provides the ```cv::aruco::CharucoBoard``` class that represent ...@@ -28,9 +28,9 @@ The aruco module provides the ```cv::aruco::CharucoBoard``` class that represent
This class, as the rest of ChArUco functionalities, are defined in: This class, as the rest of ChArUco functionalities, are defined in:
``` c++ @code{.cpp}
#include <opencv2/aruco/charuco.hpp> #include <opencv2/aruco/charuco.hpp>
``` @endcode
To define a ```CharucoBoard```, it is necesary: To define a ```CharucoBoard```, it is necesary:
...@@ -44,9 +44,9 @@ To define a ```CharucoBoard```, it is necesary: ...@@ -44,9 +44,9 @@ To define a ```CharucoBoard```, it is necesary:
As for the ```GridBoard``` objects, the aruco module provides a function to create ```CharucoBoard```s easily. This function As for the ```GridBoard``` objects, the aruco module provides a function to create ```CharucoBoard```s easily. This function
is the static function ```cv::aruco::CharucoBoard::create()``` : is the static function ```cv::aruco::CharucoBoard::create()``` :
``` c++ @code{.cpp}
cv::aruco::CharucoBoard board = cv::aruco::CharucoBoard::create(5, 7, 0.04, 0.02, dictionary); cv::aruco::CharucoBoard board = cv::aruco::CharucoBoard::create(5, 7, 0.04, 0.02, dictionary);
``` @endcode
- The first and second parameters are the number of squares in X and Y direction respectively. - The first and second parameters are the number of squares in X and Y direction respectively.
- The third and fourth parameters are the length of the squares and the markers respectively. They can be provided - The third and fourth parameters are the length of the squares and the markers respectively. They can be provided
...@@ -57,13 +57,13 @@ The ids of each of the markers are assigned by default in ascending order and st ...@@ -57,13 +57,13 @@ The ids of each of the markers are assigned by default in ascending order and st
This can be easily customized by accessing to the ids vector through ```board.ids```, like in the ```Board``` parent class. This can be easily customized by accessing to the ids vector through ```board.ids```, like in the ```Board``` parent class.
Once we have our ```CharucoBoard``` object, we can create an image to print it. This can be done with the Once we have our ```CharucoBoard``` object, we can create an image to print it. This can be done with the
```CharucoBoard::draw()``` method: <code>CharucoBoard::draw()</code> method:
``` c++ @code{.cpp}
cv::Ptr<cv::aruco::CharucoBoard> board = cv::aruco::CharucoBoard::create(5, 7, 0.04, 0.02, dictionary); cv::Ptr<cv::aruco::CharucoBoard> board = cv::aruco::CharucoBoard::create(5, 7, 0.04, 0.02, dictionary);
cv::Mat boardImage; cv::Mat boardImage;
board->draw( cv::Size(600, 500), boardImage, 10, 1 ); board->draw( cv::Size(600, 500), boardImage, 10, 1 );
``` @endcode
- The first parameter is the size of the output image in pixels. In this case 600x500 pixels. If this is not proportional - The first parameter is the size of the output image in pixels. In this case 600x500 pixels. If this is not proportional
to the board dimensions, it will be centered on the image. to the board dimensions, it will be centered on the image.
...@@ -79,9 +79,9 @@ The output image will be something like this: ...@@ -79,9 +79,9 @@ The output image will be something like this:
A full working example is included in the ```create_board_charuco.cpp``` inside the module samples folder. A full working example is included in the ```create_board_charuco.cpp``` inside the module samples folder.
Note: The samples now take input via commandline via the [OpenCV Commandline Parser](http://docs.opencv.org/trunk/d0/d2e/classcv_1_1CommandLineParser.html#gsc.tab=0). For this file the example parameters will look like Note: The samples now take input via commandline via the [OpenCV Commandline Parser](http://docs.opencv.org/trunk/d0/d2e/classcv_1_1CommandLineParser.html#gsc.tab=0). For this file the example parameters will look like
``` c++ @code{.cpp}
"_ output path_/chboard.png" -w=5 -h=7 -sl=200 -ml=120 -d=10 "_ output path_/chboard.png" -w=5 -h=7 -sl=200 -ml=120 -d=10
``` @endcode
ChArUco Board Detection ChArUco Board Detection
...@@ -102,7 +102,7 @@ ChArUco corners are interpolated from markers. ...@@ -102,7 +102,7 @@ ChArUco corners are interpolated from markers.
The function that detect the ChArUco corners is ```cv::aruco::interpolateCornersCharuco()``` . This example shows the whole process. First, markers are detected, and then the ChArUco corners are interpolated from these markers. The function that detect the ChArUco corners is ```cv::aruco::interpolateCornersCharuco()``` . This example shows the whole process. First, markers are detected, and then the ChArUco corners are interpolated from these markers.
``` c++ @code{.cpp}
cv::Mat inputImage; cv::Mat inputImage;
cv::Mat cameraMatrix, distCoeffs; cv::Mat cameraMatrix, distCoeffs;
// camera parameters are read from somewhere // camera parameters are read from somewhere
...@@ -120,7 +120,7 @@ The function that detect the ChArUco corners is ```cv::aruco::interpolateCorners ...@@ -120,7 +120,7 @@ The function that detect the ChArUco corners is ```cv::aruco::interpolateCorners
std::vector<int> charucoIds; std::vector<int> charucoIds;
cv::aruco::interpolateCornersCharuco(markerCorners, markerIds, inputImage, board, charucoCorners, charucoIds, cameraMatrix, distCoeffs); cv::aruco::interpolateCornersCharuco(markerCorners, markerIds, inputImage, board, charucoCorners, charucoIds, cameraMatrix, distCoeffs);
} }
``` @endcode
The parameters of the ```interpolateCornersCharuco()``` function are: The parameters of the ```interpolateCornersCharuco()``` function are:
- ```markerCorners``` and ```markerIds```: the detected markers from ```detectMarkers()``` function. - ```markerCorners``` and ```markerIds```: the detected markers from ```detectMarkers()``` function.
...@@ -134,7 +134,7 @@ in the ChArUco corners. ...@@ -134,7 +134,7 @@ in the ChArUco corners.
In this case, we have call ```interpolateCornersCharuco()``` providing the camera calibration parameters. However these parameters In this case, we have call ```interpolateCornersCharuco()``` providing the camera calibration parameters. However these parameters
are optional. A similar example without these parameters would be: are optional. A similar example without these parameters would be:
``` c++ @code{.cpp}
cv::Mat inputImage; cv::Mat inputImage;
cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250); cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);
cv::Ptr<cv::aruco::CharucoBoard> board = cv::aruco::CharucoBoard::create(5, 7, 0.04, 0.02, dictionary); cv::Ptr<cv::aruco::CharucoBoard> board = cv::aruco::CharucoBoard::create(5, 7, 0.04, 0.02, dictionary);
...@@ -151,7 +151,7 @@ are optional. A similar example without these parameters would be: ...@@ -151,7 +151,7 @@ are optional. A similar example without these parameters would be:
std::vector<int> charucoIds; std::vector<int> charucoIds;
cv::aruco::interpolateCornersCharuco(markerCorners, markerIds, inputImage, board, charucoCorners, charucoIds); cv::aruco::interpolateCornersCharuco(markerCorners, markerIds, inputImage, board, charucoCorners, charucoIds);
} }
``` @endcode
If calibration parameters are provided, the ChArUco corners are interpolated by, first, estimating a rough pose from the ArUco markers If calibration parameters are provided, the ChArUco corners are interpolated by, first, estimating a rough pose from the ArUco markers
and, then, reprojecting the ChArUco corners back to the image. and, then, reprojecting the ChArUco corners back to the image.
...@@ -176,9 +176,9 @@ After the ChArUco corners have been interpolated, a subpixel refinement is perfo ...@@ -176,9 +176,9 @@ After the ChArUco corners have been interpolated, a subpixel refinement is perfo
Once we have interpolated the ChArUco corners, we would probably want to draw them to see if their detections are correct. Once we have interpolated the ChArUco corners, we would probably want to draw them to see if their detections are correct.
This can be easily done using the ```drawDetectedCornersCharuco()``` function: This can be easily done using the ```drawDetectedCornersCharuco()``` function:
``` c++ @code{.cpp}
cv::aruco::drawDetectedCornersCharuco(image, charucoCorners, charucoIds, color); cv::aruco::drawDetectedCornersCharuco(image, charucoCorners, charucoIds, color);
``` @endcode
- ```image``` is the image where the corners will be drawn (it will normally be the same image where the corners were detected). - ```image``` is the image where the corners will be drawn (it will normally be the same image where the corners were detected).
- The ```outputImage``` will be a clone of ```inputImage``` with the corners drawn. - The ```outputImage``` will be a clone of ```inputImage``` with the corners drawn.
...@@ -199,7 +199,7 @@ In the presence of occlusion. like in the following image, although some corners ...@@ -199,7 +199,7 @@ In the presence of occlusion. like in the following image, although some corners
Finally, this is a full example of ChArUco detection (without using calibration parameters): Finally, this is a full example of ChArUco detection (without using calibration parameters):
``` c++ @code{.cpp}
cv::VideoCapture inputVideo; cv::VideoCapture inputVideo;
inputVideo.open(0); inputVideo.open(0);
...@@ -235,7 +235,7 @@ Finally, this is a full example of ChArUco detection (without using calibration ...@@ -235,7 +235,7 @@ Finally, this is a full example of ChArUco detection (without using calibration
if (key == 27) if (key == 27)
break; break;
} }
``` @endcode
Sample video: Sample video:
...@@ -246,9 +246,9 @@ Sample video: ...@@ -246,9 +246,9 @@ Sample video:
A full working example is included in the ```detect_board_charuco.cpp``` inside the module samples folder. A full working example is included in the ```detect_board_charuco.cpp``` inside the module samples folder.
Note: The samples now take input via commandline via the [OpenCV Commandline Parser](http://docs.opencv.org/trunk/d0/d2e/classcv_1_1CommandLineParser.html#gsc.tab=0). For this file the example parameters will look like Note: The samples now take input via commandline via the [OpenCV Commandline Parser](http://docs.opencv.org/trunk/d0/d2e/classcv_1_1CommandLineParser.html#gsc.tab=0). For this file the example parameters will look like
``` c++ @code{.cpp}
-c="_path_/calib.txt" -dp="_path_/detector_params.yml" -w=5 -h=7 -sl=0.04 -ml=0.02 -d=10 -c="_path_/calib.txt" -dp="_path_/detector_params.yml" -w=5 -h=7 -sl=0.04 -ml=0.02 -d=10
``` @endcode
ChArUco Pose Estimation ChArUco Pose Estimation
------ ------
...@@ -260,9 +260,9 @@ of the ```CharucoBoard``` is placed in the board plane with the Z axis pointing ...@@ -260,9 +260,9 @@ of the ```CharucoBoard``` is placed in the board plane with the Z axis pointing
The function for pose estimation is ```estimatePoseCharucoBoard()```: The function for pose estimation is ```estimatePoseCharucoBoard()```:
``` c++ @code{.cpp}
cv::aruco::estimatePoseCharucoBoard(charucoCorners, charucoIds, board, cameraMatrix, distCoeffs, rvec, tvec); cv::aruco::estimatePoseCharucoBoard(charucoCorners, charucoIds, board, cameraMatrix, distCoeffs, rvec, tvec);
``` @endcode
- The ```charucoCorners``` and ```charucoIds``` parameters are the detected charuco corners from the ```interpolateCornersCharuco()``` - The ```charucoCorners``` and ```charucoIds``` parameters are the detected charuco corners from the ```interpolateCornersCharuco()```
function. function.
...@@ -278,7 +278,7 @@ The axis can be drawn using ```drawAxis()``` to check the pose is correctly esti ...@@ -278,7 +278,7 @@ The axis can be drawn using ```drawAxis()``` to check the pose is correctly esti
A full example of ChArUco detection with pose estimation: A full example of ChArUco detection with pose estimation:
``` c++ @code{.cpp}
cv::VideoCapture inputVideo; cv::VideoCapture inputVideo;
inputVideo.open(0); inputVideo.open(0);
...@@ -319,11 +319,11 @@ A full example of ChArUco detection with pose estimation: ...@@ -319,11 +319,11 @@ A full example of ChArUco detection with pose estimation:
if (key == 27) if (key == 27)
break; break;
} }
``` @endcode
A full working example is included in the ```detect_board_charuco.cpp``` inside the module samples folder. A full working example is included in the ```detect_board_charuco.cpp``` inside the module samples folder.
Note: The samples now take input via commandline via the [OpenCV Commandline Parser](http://docs.opencv.org/trunk/d0/d2e/classcv_1_1CommandLineParser.html#gsc.tab=0). For this file the example parameters will look like Note: The samples now take input via commandline via the [OpenCV Commandline Parser](http://docs.opencv.org/trunk/d0/d2e/classcv_1_1CommandLineParser.html#gsc.tab=0). For this file the example parameters will look like
``` c++ @code{.cpp}
"_path_/calib.txt" -dp="_path_/detector_params.yml" -w=5 -h=7 -sl=0.04 -ml=0.02 -d=10 "_path_/calib.txt" -dp="_path_/detector_params.yml" -w=5 -h=7 -sl=0.04 -ml=0.02 -d=10
``` @endcode
...@@ -44,11 +44,11 @@ ChArUco Diamond Creation ...@@ -44,11 +44,11 @@ ChArUco Diamond Creation
The image of a diamond marker can be easily created using the ```drawCharucoDiamond()``` function. The image of a diamond marker can be easily created using the ```drawCharucoDiamond()``` function.
For instance: For instance:
``` c++ @code{.cpp}
cv::Mat diamondImage; cv::Mat diamondImage;
cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250); cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);
cv::aruco::drawCharucoDiamond(dictionary, cv::Vec4i(45,68,28,74), 200, 120, markerImage); cv::aruco::drawCharucoDiamond(dictionary, cv::Vec4i(45,68,28,74), 200, 120, markerImage);
``` @endcode
This will create a diamond marker image with a square size of 200 pixels and a marker size of 120 pixels. This will create a diamond marker image with a square size of 200 pixels and a marker size of 120 pixels.
The marker ids are given in the second parameter as a ```Vec4i``` object. The order of the marker ids The marker ids are given in the second parameter as a ```Vec4i``` object. The order of the marker ids
...@@ -61,9 +61,9 @@ The image produced will be: ...@@ -61,9 +61,9 @@ The image produced will be:
A full working example is included in the ```create_diamond.cpp``` inside the module samples folder. A full working example is included in the ```create_diamond.cpp``` inside the module samples folder.
Note: The samples now take input via commandline via the [OpenCV Commandline Parser](http://docs.opencv.org/trunk/d0/d2e/classcv_1_1CommandLineParser.html#gsc.tab=0). For this file the example parameters will look like Note: The samples now take input via commandline via the [OpenCV Commandline Parser](http://docs.opencv.org/trunk/d0/d2e/classcv_1_1CommandLineParser.html#gsc.tab=0). For this file the example parameters will look like
``` c++ @code{.cpp}
"_path_/mydiamond.png" -sl=200 -ml=120 -d=10 -ids=45,68,28,74 "_path_/mydiamond.png" -sl=200 -ml=120 -d=10 -ids=45,68,28,74
``` @endcode
ChArUco Diamond Detection ChArUco Diamond Detection
------ ------
...@@ -71,7 +71,7 @@ ChArUco Diamond Detection ...@@ -71,7 +71,7 @@ ChArUco Diamond Detection
As in most cases, the detection of diamond markers requires a previous detection of ArUco markers. As in most cases, the detection of diamond markers requires a previous detection of ArUco markers.
After detecting markers, diamond are detected using the ```detectCharucoDiamond()``` function: After detecting markers, diamond are detected using the ```detectCharucoDiamond()``` function:
``` c++ @code{.cpp}
cv::Mat inputImage; cv::Mat inputImage;
float squareLength = 0.40; float squareLength = 0.40;
float markerLength = 0.25; float markerLength = 0.25;
...@@ -89,7 +89,7 @@ After detecting markers, diamond are detected using the ```detectCharucoDiamond( ...@@ -89,7 +89,7 @@ After detecting markers, diamond are detected using the ```detectCharucoDiamond(
// detect diamon diamonds // detect diamon diamonds
cv::aruco::detectCharucoDiamond(inputImage, markerCorners, markerIds, squareLength / markerLength, diamondCorners, diamondIds); cv::aruco::detectCharucoDiamond(inputImage, markerCorners, markerIds, squareLength / markerLength, diamondCorners, diamondIds);
``` @endcode
The ```detectCharucoDiamond()``` function receives the original image and the previous detected marker corners and ids. The ```detectCharucoDiamond()``` function receives the original image and the previous detected marker corners and ids.
The input image is necessary to perform subpixel refinement in the ChArUco corners. The input image is necessary to perform subpixel refinement in the ChArUco corners.
...@@ -105,14 +105,14 @@ diamond corners in ```diamondCorners```. Each id is actually an array of 4 integ ...@@ -105,14 +105,14 @@ diamond corners in ```diamondCorners```. Each id is actually an array of 4 integ
The detected diamond can be visualized using the function ```drawDetectedDiamonds()``` which simply recieves the image and the diamond The detected diamond can be visualized using the function ```drawDetectedDiamonds()``` which simply recieves the image and the diamond
corners and ids: corners and ids:
``` c++ @code{.cpp}
... ...
std::vector<cv::Vec4i> diamondIds; std::vector<cv::Vec4i> diamondIds;
std::vector<std::vector<cv::Point2f>> diamondCorners; std::vector<std::vector<cv::Point2f>> diamondCorners;
cv::aruco::detectCharucoDiamond(inputImage, markerCorners, markerIds, squareLength / markerLength, diamondCorners, diamondIds); cv::aruco::detectCharucoDiamond(inputImage, markerCorners, markerIds, squareLength / markerLength, diamondCorners, diamondIds);
cv::aruco::drawDetectedDiamonds(inputImage, diamondCorners, diamondIds); cv::aruco::drawDetectedDiamonds(inputImage, diamondCorners, diamondIds);
``` @endcode
The result is the same that the one produced by ```drawDetectedMarkers()```, but printing the four ids of the diamond: The result is the same that the one produced by ```drawDetectedMarkers()```, but printing the four ids of the diamond:
...@@ -121,9 +121,9 @@ The result is the same that the one produced by ```drawDetectedMarkers()```, but ...@@ -121,9 +121,9 @@ The result is the same that the one produced by ```drawDetectedMarkers()```, but
A full working example is included in the ```detect_diamonds.cpp``` inside the module samples folder. A full working example is included in the ```detect_diamonds.cpp``` inside the module samples folder.
Note: The samples now take input via commandline via the [OpenCV Commandline Parser](http://docs.opencv.org/trunk/d0/d2e/classcv_1_1CommandLineParser.html#gsc.tab=0). For this file the example parameters will look like Note: The samples now take input via commandline via the [OpenCV Commandline Parser](http://docs.opencv.org/trunk/d0/d2e/classcv_1_1CommandLineParser.html#gsc.tab=0). For this file the example parameters will look like
``` c++ @code{.cpp}
-c="_path_/calib.txt" -dp="_path_/detector_params.yml" -sl=0.04 -ml=0.02 -d=10 -c="_path_/calib.txt" -dp="_path_/detector_params.yml" -sl=0.04 -ml=0.02 -d=10
``` @endcode
ChArUco Diamond Pose Estimation ChArUco Diamond Pose Estimation
------ ------
...@@ -131,7 +131,7 @@ ChArUco Diamond Pose Estimation ...@@ -131,7 +131,7 @@ ChArUco Diamond Pose Estimation
Since a ChArUco diamond is represented by its four corners, its pose can be estimated in the same way than in a single ArUco marker, Since a ChArUco diamond is represented by its four corners, its pose can be estimated in the same way than in a single ArUco marker,
i.e. using the ```estimatePoseSingleMarkers()``` function. For instance: i.e. using the ```estimatePoseSingleMarkers()``` function. For instance:
``` c++ @code{.cpp}
... ...
std::vector<cv::Vec4i> diamondIds; std::vector<cv::Vec4i> diamondIds;
...@@ -147,7 +147,7 @@ i.e. using the ```estimatePoseSingleMarkers()``` function. For instance: ...@@ -147,7 +147,7 @@ i.e. using the ```estimatePoseSingleMarkers()``` function. For instance:
// draw axis // draw axis
for(unsigned int i=0; i<rvecs.size(); i++) for(unsigned int i=0; i<rvecs.size(); i++)
cv::aruco::drawAxis(inputImage, camMatrix, distCoeffs, rvecs[i], tvecs[i], axisLength); cv::aruco::drawAxis(inputImage, camMatrix, distCoeffs, rvecs[i], tvecs[i], axisLength);
``` @endcode
The function will obtain the rotation and translation vector for each of the diamond marker and store them The function will obtain the rotation and translation vector for each of the diamond marker and store them
in ```rvecs``` and ```tvecs```. Note that the diamond corners are a chessboard square corners and thus, the square length in ```rvecs``` and ```tvecs```. Note that the diamond corners are a chessboard square corners and thus, the square length
...@@ -169,6 +169,6 @@ Sample video: ...@@ -169,6 +169,6 @@ Sample video:
A full working example is included in the ```detect_diamonds.cpp``` inside the module samples folder. A full working example is included in the ```detect_diamonds.cpp``` inside the module samples folder.
Note: The samples now take input via commandline via the [OpenCV Commandline Parser](http://docs.opencv.org/trunk/d0/d2e/classcv_1_1CommandLineParser.html#gsc.tab=0). For this file the example parameters will look like Note: The samples now take input via commandline via the [OpenCV Commandline Parser](http://docs.opencv.org/trunk/d0/d2e/classcv_1_1CommandLineParser.html#gsc.tab=0). For this file the example parameters will look like
``` c++ @code{.cpp}
-c="_output path_/calib.txt" -dp="_path_/detector_params.yml" -sl=0.04 -ml=0.02 -d=10 -c="_output path_/calib.txt" -dp="_path_/detector_params.yml" -sl=0.04 -ml=0.02 -d=10
``` @endcode
...@@ -67,11 +67,6 @@ class CV_EXPORTS_W Saliency : public virtual Algorithm ...@@ -67,11 +67,6 @@ class CV_EXPORTS_W Saliency : public virtual Algorithm
*/ */
virtual ~Saliency(); virtual ~Saliency();
/**
* \brief Create Saliency by saliency type.
*/
static Ptr<Saliency> create( const String& saliencyType );
/** /**
* \brief Compute the saliency * \brief Compute the saliency
* \param image The image. * \param image The image.
...@@ -80,12 +75,6 @@ class CV_EXPORTS_W Saliency : public virtual Algorithm ...@@ -80,12 +75,6 @@ class CV_EXPORTS_W Saliency : public virtual Algorithm
*/ */
CV_WRAP bool computeSaliency( InputArray image, OutputArray saliencyMap ); CV_WRAP bool computeSaliency( InputArray image, OutputArray saliencyMap );
/**
* \brief Get the name of the specific saliency type
* \return The name of the tracker initializer
*/
CV_WRAP String getClassName() const;
protected: protected:
virtual bool computeSaliencyImpl( InputArray image, OutputArray saliencyMap ) = 0; virtual bool computeSaliencyImpl( InputArray image, OutputArray saliencyMap ) = 0;
......
...@@ -234,15 +234,28 @@ private: ...@@ -234,15 +234,28 @@ private:
bool templateOrdering(); bool templateOrdering();
bool templateReplacement( const Mat& finalBFMask, const Mat& image ); bool templateReplacement( const Mat& finalBFMask, const Mat& image );
// Decision threshold adaptation and Activity control function
bool activityControl(const Mat& current_noisePixelsMask);
bool decisionThresholdAdaptation();
// changing structure // changing structure
std::vector<Ptr<Mat> > backgroundModel;// The vector represents the background template T0---TK of reference paper. std::vector<Ptr<Mat> > backgroundModel;// The vector represents the background template T0---TK of reference paper.
// Matrices are two-channel matrix. In the first layer there are the B (background value) // Matrices are two-channel matrix. In the first layer there are the B (background value)
// for each pixel. In the second layer, there are the C (efficacy) value for each pixel // for each pixel. In the second layer, there are the C (efficacy) value for each pixel
Mat potentialBackground;// Two channel Matrix. For each pixel, in the first level there are the Ba value (potential background value) Mat potentialBackground;// Two channel Matrix. For each pixel, in the first level there are the Ba value (potential background value)
// and in the secon level there are the Ca value, the counter for each potential value. // and in the secon level there are the Ca value, the counter for each potential value.
Mat epslonPixelsValue; // epslon threshold Mat epslonPixelsValue;// epslon threshold
Mat activityPixelsValue;// Activity level of each pixel
//vector<Mat> noisePixelMask; // We define a ‘noise-pixel’ as a pixel that has been classified as a foreground pixel during the full resolution
Mat noisePixelMask;// We define a ‘noise-pixel’ as a pixel that has been classified as a foreground pixel during the full resolution
//detection process,however, after the low resolution detection, it has become a
// background pixel. The matrix is two-channel matrix. In the first layer there is the mask ( the identified noise-pixels are set to 1 while other pixels are 0)
// for each pixel. In the second layer, there is the value of activity level A for each pixel.
//fixed parameter //fixed parameter
bool activityControlFlag;
bool neighborhoodCheck; bool neighborhoodCheck;
int N_DS;// Number of template to be downsampled and used in lowResolutionDetection function int N_DS;// Number of template to be downsampled and used in lowResolutionDetection function
CV_PROP_RW int imageWidth;// Width of input image CV_PROP_RW int imageWidth;// Width of input image
...@@ -257,6 +270,13 @@ private: ...@@ -257,6 +270,13 @@ private:
// long-term template, regardless of any subsequent background changes. A relatively large (eg gamma=3) will // long-term template, regardless of any subsequent background changes. A relatively large (eg gamma=3) will
//restrain the generation of ghosts. //restrain the generation of ghosts.
uchar Ainc;// Activity Incrementation;
int Bmax;// Upper-bound value for pixel activity
int Bth;// Max activity threshold
int Binc, Bdec;// Threshold for pixel-level decision threshold (epslon) adaptation
float deltaINC, deltaDEC;// Increment-decrement value for epslon adaptation
int epslonMIN, epslonMAX;// Range values for epslon threshold
}; };
/************************************ Specific Objectness Specialized Classes ************************************/ /************************************ Specific Objectness Specialized Classes ************************************/
...@@ -417,7 +437,7 @@ private: ...@@ -417,7 +437,7 @@ private:
int _Clr;// int _Clr;//
static const char* _clrName[3]; static const char* _clrName[3];
// Names and paths to read model and to store results // Names and paths to read model and to store results
std::string _modelName, _bbResDir, _trainingPath, _resultsDir; std::string _modelName, _bbResDir, _trainingPath, _resultsDir;
std::vector<int> _svmSzIdxs;// Indexes of active size. It's equal to _svmFilters.size() and _svmReW1f.rows std::vector<int> _svmSzIdxs;// Indexes of active size. It's equal to _svmFilters.size() and _svmReW1f.rows
...@@ -425,12 +445,12 @@ private: ...@@ -425,12 +445,12 @@ private:
FilterTIG _tigF;// TIG filter FilterTIG _tigF;// TIG filter
Mat _svmReW1f;// Re-weight parameters learned at stage II. Mat _svmReW1f;// Re-weight parameters learned at stage II.
// List of the rectangles' objectness value, in the same order as // List of the rectangles' objectness value, in the same order as
// the vector<Vec4i> objectnessBoundingBox returned by the algorithm (in computeSaliencyImpl function) // the vector<Vec4i> objectnessBoundingBox returned by the algorithm (in computeSaliencyImpl function)
std::vector<float> objectnessValues; std::vector<float> objectnessValues;
private: private:
// functions // functions
inline static float LoG( float x, float y, float delta ) inline static float LoG( float x, float y, float delta )
{ {
...@@ -438,17 +458,17 @@ private: ...@@ -438,17 +458,17 @@ private:
return -1.0f / ( (float) ( CV_PI ) * pow( delta, 4 ) ) * ( 1 + d ) * exp( d ); return -1.0f / ( (float) ( CV_PI ) * pow( delta, 4 ) ) * ( 1 + d ) * exp( d );
} // Laplacian of Gaussian } // Laplacian of Gaussian
// Read matrix from binary file // Read matrix from binary file
static bool matRead( const std::string& filename, Mat& M ); static bool matRead( const std::string& filename, Mat& M );
void setColorSpace( int clr = MAXBGR ); void setColorSpace( int clr = MAXBGR );
// Load trained model. // Load trained model.
int loadTrainedModel( std::string modelName = "" );// Return -1, 0, or 1 if partial, none, or all loaded int loadTrainedModel( std::string modelName = "" );// Return -1, 0, or 1 if partial, none, or all loaded
// Get potential bounding boxes, each of which is represented by a Vec4i for (minX, minY, maxX, maxY). // Get potential bounding boxes, each of which is represented by a Vec4i for (minX, minY, maxX, maxY).
// The trained model should be prepared before calling this function: loadTrainedModel() or trainStageI() + trainStageII(). // The trained model should be prepared before calling this function: loadTrainedModel() or trainStageI() + trainStageII().
// Use numDet to control the final number of proposed bounding boxes, and number of per size (scale and aspect ratio) // Use numDet to control the final number of proposed bounding boxes, and number of per size (scale and aspect ratio)
void getObjBndBoxes( Mat &img3u, ValStructVec<float, Vec4i> &valBoxes, int numDetPerSize = 120 ); void getObjBndBoxes( Mat &img3u, ValStructVec<float, Vec4i> &valBoxes, int numDetPerSize = 120 );
void getObjBndBoxesForSingleImage( Mat img, ValStructVec<float, Vec4i> &boxes, int numDetPerSize ); void getObjBndBoxesForSingleImage( Mat img, ValStructVec<float, Vec4i> &boxes, int numDetPerSize );
...@@ -460,7 +480,7 @@ private: ...@@ -460,7 +480,7 @@ private:
void predictBBoxSI( Mat &mag3u, ValStructVec<float, Vec4i> &valBoxes, std::vector<int> &sz, int NUM_WIN_PSZ = 100, bool fast = true ); void predictBBoxSI( Mat &mag3u, ValStructVec<float, Vec4i> &valBoxes, std::vector<int> &sz, int NUM_WIN_PSZ = 100, bool fast = true );
void predictBBoxSII( ValStructVec<float, Vec4i> &valBoxes, const std::vector<int> &sz ); void predictBBoxSII( ValStructVec<float, Vec4i> &valBoxes, const std::vector<int> &sz );
// Calculate the image gradient: center option as in VLFeat // Calculate the image gradient: center option as in VLFeat
void gradientMag( Mat &imgBGR3u, Mat &mag1u ); void gradientMag( Mat &imgBGR3u, Mat &mag1u );
static void gradientRGB( Mat &bgr3u, Mat &mag1u ); static void gradientRGB( Mat &bgr3u, Mat &mag1u );
...@@ -479,7 +499,7 @@ private: ...@@ -479,7 +499,7 @@ private:
return abs( u[0] - v[0] ) + abs( u[1] - v[1] ) + abs( u[2] - v[2] ); return abs( u[0] - v[0] ) + abs( u[1] - v[1] ) + abs( u[2] - v[2] );
} }
//Non-maximal suppress //Non-maximal suppress
static void nonMaxSup( Mat &matchCost1f, ValStructVec<float, Point> &matchCost, int NSS = 1, int maxPoint = 50, bool fast = true ); static void nonMaxSup( Mat &matchCost1f, ValStructVec<float, Point> &matchCost, int NSS = 1, int maxPoint = 50, bool fast = true );
}; };
......
...@@ -64,6 +64,7 @@ static void help() ...@@ -64,6 +64,7 @@ static void help()
int main( int argc, char** argv ) int main( int argc, char** argv )
{ {
CommandLineParser parser( argc, argv, keys ); CommandLineParser parser( argc, argv, keys );
String saliency_algorithm = parser.get<String>( 0 ); String saliency_algorithm = parser.get<String>( 0 );
...@@ -94,13 +95,7 @@ int main( int argc, char** argv ) ...@@ -94,13 +95,7 @@ int main( int argc, char** argv )
Mat frame; Mat frame;
//instantiates the specific Saliency //instantiates the specific Saliency
Ptr<Saliency> saliencyAlgorithm = Saliency::create( saliency_algorithm ); Ptr<Saliency> saliencyAlgorithm;
if( saliencyAlgorithm == NULL )
{
cout << "***Error in the instantiation of the saliency algorithm...***\n";
return -1;
}
Mat binaryMap; Mat binaryMap;
Mat image; Mat image;
...@@ -116,6 +111,7 @@ int main( int argc, char** argv ) ...@@ -116,6 +111,7 @@ int main( int argc, char** argv )
if( saliency_algorithm.find( "SPECTRAL_RESIDUAL" ) == 0 ) if( saliency_algorithm.find( "SPECTRAL_RESIDUAL" ) == 0 )
{ {
Mat saliencyMap; Mat saliencyMap;
saliencyAlgorithm = StaticSaliencySpectralResidual::create();
if( saliencyAlgorithm->computeSaliency( image, saliencyMap ) ) if( saliencyAlgorithm->computeSaliency( image, saliencyMap ) )
{ {
StaticSaliencySpectralResidual spec; StaticSaliencySpectralResidual spec;
...@@ -131,6 +127,7 @@ int main( int argc, char** argv ) ...@@ -131,6 +127,7 @@ int main( int argc, char** argv )
else if( saliency_algorithm.find( "FINE_GRAINED" ) == 0 ) else if( saliency_algorithm.find( "FINE_GRAINED" ) == 0 )
{ {
Mat saliencyMap; Mat saliencyMap;
saliencyAlgorithm = StaticSaliencyFineGrained::create();
if( saliencyAlgorithm->computeSaliency( image, saliencyMap ) ) if( saliencyAlgorithm->computeSaliency( image, saliencyMap ) )
{ {
imshow( "Saliency Map", saliencyMap ); imshow( "Saliency Map", saliencyMap );
...@@ -150,6 +147,7 @@ int main( int argc, char** argv ) ...@@ -150,6 +147,7 @@ int main( int argc, char** argv )
else else
{ {
saliencyAlgorithm = ObjectnessBING::create();
vector<Vec4i> saliencyMap; vector<Vec4i> saliencyMap;
saliencyAlgorithm.dynamicCast<ObjectnessBING>()->setTrainingPath( training_path ); saliencyAlgorithm.dynamicCast<ObjectnessBING>()->setTrainingPath( training_path );
saliencyAlgorithm.dynamicCast<ObjectnessBING>()->setBBResDir( training_path + "/Results" ); saliencyAlgorithm.dynamicCast<ObjectnessBING>()->setBBResDir( training_path + "/Results" );
...@@ -163,8 +161,7 @@ int main( int argc, char** argv ) ...@@ -163,8 +161,7 @@ int main( int argc, char** argv )
} }
else if( saliency_algorithm.find( "BinWangApr2014" ) == 0 ) else if( saliency_algorithm.find( "BinWangApr2014" ) == 0 )
{ {
saliencyAlgorithm = MotionSaliencyBinWangApr2014::create();
//Ptr<Size> size = Ptr<Size>( new Size( image.cols, image.rows ) );
saliencyAlgorithm.dynamicCast<MotionSaliencyBinWangApr2014>()->setImagesize( image.cols, image.rows ); saliencyAlgorithm.dynamicCast<MotionSaliencyBinWangApr2014>()->setImagesize( image.cols, image.rows );
saliencyAlgorithm.dynamicCast<MotionSaliencyBinWangApr2014>()->init(); saliencyAlgorithm.dynamicCast<MotionSaliencyBinWangApr2014>()->init();
...@@ -175,13 +172,14 @@ int main( int argc, char** argv ) ...@@ -175,13 +172,14 @@ int main( int argc, char** argv )
{ {
cap >> frame; cap >> frame;
if( frame.empty() )
{
return 0;
}
cvtColor( frame, frame, COLOR_BGR2GRAY ); cvtColor( frame, frame, COLOR_BGR2GRAY );
Mat saliencyMap; Mat saliencyMap;
if( saliencyAlgorithm->computeSaliency( frame, saliencyMap ) ) saliencyAlgorithm->computeSaliency( frame, saliencyMap );
{
std::cout << "current frame motion saliency done" << std::endl;
}
imshow( "image", frame ); imshow( "image", frame );
imshow( "saliencyMap", saliencyMap * 255 ); imshow( "saliencyMap", saliencyMap * 255 );
......
...@@ -41,11 +41,10 @@ ...@@ -41,11 +41,10 @@
#include <limits> #include <limits>
#include "precomp.hpp" #include "precomp.hpp"
//TODO delete highgui include
//#include <opencv2/highgui.hpp>
#define thetaA_VAL 200 #define thetaA_VAL 200
#define thetaL_VAL 250 #define thetaL_VAL 250
#define epslonGeneric 20
namespace cv namespace cv
{ {
...@@ -71,19 +70,25 @@ MotionSaliencyBinWangApr2014::MotionSaliencyBinWangApr2014() ...@@ -71,19 +70,25 @@ MotionSaliencyBinWangApr2014::MotionSaliencyBinWangApr2014()
gamma = 3; gamma = 3;
neighborhoodCheck = true; neighborhoodCheck = true;
Ainc = 6; // Activity Incrementation;
Bmax = 80; // Upper-bound value for pixel activity
Bth = 20; //70; // Max activity threshold
Binc = 15; //50;
Bdec = 5; //20; // Threshold for pixel-level decision threshold (epslon) adaptation
deltaINC = 20;
deltaDEC = 0.125; // Increment-decrement value for epslon adaptation
epslonMIN = 18;
epslonMAX = 80;
className = "BinWangApr2014"; className = "BinWangApr2014";
} }
bool MotionSaliencyBinWangApr2014::init() bool MotionSaliencyBinWangApr2014::init()
{ {
activityControlFlag = false;
Size imgSize( imageWidth, imageHeight ); Size imgSize( imageWidth, imageHeight );
epslonPixelsValue = Mat( imgSize.height, imgSize.width, CV_32F, Scalar( 20 ) ); epslonPixelsValue = Mat( imgSize.height, imgSize.width, CV_32F, Scalar( epslonGeneric ) );
// Median of range [18, 80] advised in reference paper. potentialBackground = Mat( imgSize.height, imgSize.width, CV_8UC2, Scalar( 0, 0 ) );
// Since data is even, the median is estimated using two values ​​that occupy
// the position (n / 2) and ((n / 2) +1) (choose their arithmetic mean).
potentialBackground = Mat( imgSize.height, imgSize.width, CV_32FC2, Scalar( std::numeric_limits<float>::quiet_NaN(), 0 ) );
backgroundModel.resize( K + 1 ); backgroundModel.resize( K + 1 );
for ( int i = 0; i < K + 1; i++ ) for ( int i = 0; i < K + 1; i++ )
...@@ -95,6 +100,11 @@ bool MotionSaliencyBinWangApr2014::init() ...@@ -95,6 +100,11 @@ bool MotionSaliencyBinWangApr2014::init()
backgroundModel[i] = tmp; backgroundModel[i] = tmp;
} }
noisePixelMask.create( imgSize.height, imgSize.width, CV_8U );
noisePixelMask.setTo( Scalar( 0 ) );
activityPixelsValue.create( imgSize.height, imgSize.width, CV_8U );
activityPixelsValue.setTo( Scalar( 0 ) );
return true; return true;
} }
...@@ -109,17 +119,17 @@ bool MotionSaliencyBinWangApr2014::fullResolutionDetection( const Mat& image2, M ...@@ -109,17 +119,17 @@ bool MotionSaliencyBinWangApr2014::fullResolutionDetection( const Mat& image2, M
{ {
Mat image = image2.clone(); Mat image = image2.clone();
float currentPixelValue; uchar currentPixelValue;
float currentEpslonValue; float currentEpslonValue;
bool backgFlag = false; bool backgFlag = false;
// Initially, all pixels are considered as foreground and then we evaluate with the background model // Initially, all pixels are considered as foreground and then we evaluate with the background model
highResBFMask.create( image.rows, image.cols, CV_32F ); highResBFMask.create( image.rows, image.cols, CV_8U );
highResBFMask.setTo( 1 ); highResBFMask.setTo( 1 );
uchar* pImage; uchar* pImage;
float* pEpslon; float* pEpslon;
float* pMask; uchar* pMask;
// Scan all pixels of image // Scan all pixels of image
for ( int i = 0; i < image.rows; i++ ) for ( int i = 0; i < image.rows; i++ )
...@@ -127,10 +137,14 @@ bool MotionSaliencyBinWangApr2014::fullResolutionDetection( const Mat& image2, M ...@@ -127,10 +137,14 @@ bool MotionSaliencyBinWangApr2014::fullResolutionDetection( const Mat& image2, M
pImage = image.ptr<uchar>( i ); pImage = image.ptr<uchar>( i );
pEpslon = epslonPixelsValue.ptr<float>( i ); pEpslon = epslonPixelsValue.ptr<float>( i );
pMask = highResBFMask.ptr<float>( i ); pMask = highResBFMask.ptr<uchar>( i );
for ( int j = 0; j < image.cols; j++ ) for ( int j = 0; j < image.cols; j++ )
{ {
/* Pixels with activity greater than Bth are eliminated from the detection result. In this way,
continuously blinking noise-pixels will be eliminated from the detection results,
preventing the generation of false positives.*/
if( activityPixelsValue.at<uchar>( i, j ) < Bth )
{
backgFlag = false; backgFlag = false;
currentPixelValue = pImage[j]; currentPixelValue = pImage[j];
currentEpslonValue = pEpslon[j]; currentEpslonValue = pEpslon[j];
...@@ -140,6 +154,8 @@ bool MotionSaliencyBinWangApr2014::fullResolutionDetection( const Mat& image2, M ...@@ -140,6 +154,8 @@ bool MotionSaliencyBinWangApr2014::fullResolutionDetection( const Mat& image2, M
{ {
counter += (int) backgroundModel[z]->ptr<Vec2f>( i )[j][1]; counter += (int) backgroundModel[z]->ptr<Vec2f>( i )[j][1];
if( counter != 0 )
break;
} }
if( counter != 0 ) //if at least the first template is activated / initialized if( counter != 0 ) //if at least the first template is activated / initialized
...@@ -183,6 +199,11 @@ bool MotionSaliencyBinWangApr2014::fullResolutionDetection( const Mat& image2, M ...@@ -183,6 +199,11 @@ bool MotionSaliencyBinWangApr2014::fullResolutionDetection( const Mat& image2, M
{ {
pMask[j] = 1; //if the model of the current pixel is not yet initialized, we mark the pixels as foreground pMask[j] = 1; //if the model of the current pixel is not yet initialized, we mark the pixels as foreground
} }
}
else
{
pMask[j] = 0;
}
} }
} // end "for" cicle of all image's pixels } // end "for" cicle of all image's pixels
...@@ -211,11 +232,11 @@ bool MotionSaliencyBinWangApr2014::lowResolutionDetection( const Mat& image, Mat ...@@ -211,11 +232,11 @@ bool MotionSaliencyBinWangApr2014::lowResolutionDetection( const Mat& image, Mat
Mat currentModel; Mat currentModel;
// Initially, all pixels are considered as foreground and then we evaluate with the background model // Initially, all pixels are considered as foreground and then we evaluate with the background model
lowResBFMask.create( image.rows, image.cols, CV_32F ); lowResBFMask.create( image.rows, image.cols, CV_8U );
lowResBFMask.setTo( 1 ); lowResBFMask.setTo( 1 );
// Scan all the ROI of original matrices // Scan all the ROI of original matrices
for ( int i = 0; i < ceil( (float) image.rows / N ); i++ ) for ( int i = 0; i < (int)ceil( (float) image.rows / N ); i++ )
{ {
if( ( roi.y + ( N - 1 ) ) <= ( image.rows - 1 ) ) if( ( roi.y + ( N - 1 ) ) <= ( image.rows - 1 ) )
{ {
...@@ -223,8 +244,14 @@ bool MotionSaliencyBinWangApr2014::lowResolutionDetection( const Mat& image, Mat ...@@ -223,8 +244,14 @@ bool MotionSaliencyBinWangApr2014::lowResolutionDetection( const Mat& image, Mat
roi = Rect( Point( roi.x, roi.y ), Size( N, N ) ); roi = Rect( Point( roi.x, roi.y ), Size( N, N ) );
} }
for ( int j = 0; j < ceil( (float) image.cols / N ); j++ ) for ( int j = 0; j < (int)ceil( (float) image.cols / N ); j++ )
{ {
/* Pixels with activity greater than Bth are eliminated from the detection result. In this way,
continuously blinking noise-pixels will be eliminated from the detection results,
preventing the generation of false positives.*/
if( activityPixelsValue.at<uchar>( i, j ) < Bth )
{
// Compute the mean of image's block and epslonMatrix's block based on ROI // Compute the mean of image's block and epslonMatrix's block based on ROI
Mat roiImage = image( roi ); Mat roiImage = image( roi );
Mat roiEpslon = epslonPixelsValue( roi ); Mat roiEpslon = epslonPixelsValue( roi );
...@@ -262,6 +289,12 @@ bool MotionSaliencyBinWangApr2014::lowResolutionDetection( const Mat& image, Mat ...@@ -262,6 +289,12 @@ bool MotionSaliencyBinWangApr2014::lowResolutionDetection( const Mat& image, Mat
roi = Rect( Point( roi.x, roi.y ), Size( abs( ( image.cols - 1 ) - roi.x ) + 1, abs( ( image.rows - 1 ) - roi.y ) + 1 ) ); roi = Rect( Point( roi.x, roi.y ), Size( abs( ( image.cols - 1 ) - roi.x ) + 1, abs( ( image.rows - 1 ) - roi.y ) + 1 ) );
} }
} }
else
{
// The correspondence pixel in the BF mask is set as background ( 0 value)
rectangle( lowResBFMask, roi, Scalar( 0 ), FILLED );
}
}
//Shift the ROI from up to down follow the block dimension, also bringing it back to beginning of row //Shift the ROI from up to down follow the block dimension, also bringing it back to beginning of row
roi.x = 0; roi.x = 0;
roi.y += N; roi.y += N;
...@@ -275,7 +308,7 @@ bool MotionSaliencyBinWangApr2014::lowResolutionDetection( const Mat& image, Mat ...@@ -275,7 +308,7 @@ bool MotionSaliencyBinWangApr2014::lowResolutionDetection( const Mat& image, Mat
} }
else else
{ {
lowResBFMask.create( image.rows, image.cols, CV_32F ); lowResBFMask.create( image.rows, image.cols, CV_8U );
lowResBFMask.setTo( 1 ); lowResBFMask.setTo( 1 );
return false; return false;
} }
...@@ -289,58 +322,70 @@ bool inline pairCompare( std::pair<float, float> t, std::pair<float, float> t_pl ...@@ -289,58 +322,70 @@ bool inline pairCompare( std::pair<float, float> t, std::pair<float, float> t_pl
} }
// Background model maintenance functions
bool MotionSaliencyBinWangApr2014::templateOrdering() bool MotionSaliencyBinWangApr2014::templateOrdering()
{ {
std::vector<std::pair<float, float> > pixelTemplates( backgroundModel.size() );
Vec2f* bgModel_0P; Mat dstMask, tempMat, dstMask2, dstMask3;
Vec2f* bgModel_1P; Mat convertMat1, convertMat2;
int backGroundModelSize = (int)backgroundModel.size();
// Scan all pixels of image std::vector<std::vector<Mat> > channelSplit( backGroundModelSize );
for ( int i = 0; i < backgroundModel[0]->rows; i++ ) for ( int i = 0; i < backGroundModelSize; i++ )
{ {
bgModel_0P = backgroundModel[0]->ptr<Vec2f>( i ); split( *backgroundModel[i], channelSplit[i] );
bgModel_1P = backgroundModel[1]->ptr<Vec2f>( i );
for ( int j = 0; j < backgroundModel[0]->cols; j++ ) }
//Bubble sort : Template T1 - Tk
for ( int i = 1; i < backGroundModelSize - 1; i++ )
{ {
// scan background model vector from T1 to Tk // compare and order the i-th template with the others
for ( size_t z = 1; z < backgroundModel.size(); z++ ) for ( int j = i + 1; j < backGroundModelSize; j++ )
{ {
Vec2f bgModel_zP = backgroundModel[z]->ptr<Vec2f>( i )[j];
// Fill vector of pairs
pixelTemplates[z - 1].first = bgModel_zP[0]; // Current B (background value)
pixelTemplates[z - 1].second = bgModel_zP[1]; // Current C (efficacy value)
}
//SORT template from T1 to Tk compare( channelSplit[j][1], channelSplit[i][1], dstMask, CMP_GT );
std::sort( pixelTemplates.begin(), pixelTemplates.end(), pairCompare );
//REFILL CURRENT MODEL ( T1...Tk) channelSplit[i][0].copyTo( tempMat );
for ( size_t zz = 1; zz < backgroundModel.size(); zz++ ) channelSplit[j][0].copyTo( channelSplit[i][0], dstMask );
{ tempMat.copyTo( channelSplit[j][0], dstMask );
backgroundModel[zz]->ptr<Vec2f>( i )[j][0] = pixelTemplates[zz - 1].first; // Replace previous B with new ordered B value
backgroundModel[zz]->ptr<Vec2f>( i )[j][1] = pixelTemplates[zz - 1].second; // Replace previous C with new ordered C value channelSplit[i][1].copyTo( tempMat );
channelSplit[j][1].copyTo( channelSplit[i][1], dstMask );
tempMat.copyTo( channelSplit[j][1], dstMask );
}
} }
// SORT Template T0 and T1 // SORT Template T0 and T1
if( bgModel_1P[j][1] > thetaL && bgModel_0P[j][1] < thetaL ) Mat M_deltaL( backgroundModel[0]->rows, backgroundModel[0]->cols, CV_32F, Scalar( thetaL ) );
{
// swap B value of T0 with B value of T1 (for current model) compare( channelSplit[1][1], M_deltaL, dstMask2, CMP_GT );
swap( bgModel_0P[j][0], bgModel_1P[j][0] ); compare( M_deltaL, channelSplit[0][1], dstMask3, CMP_GT );
// set new C0 value for current model) threshold( dstMask2, dstMask2, 0, 1, THRESH_BINARY );
swap( bgModel_0P[j][1], bgModel_1P[j][1] ); threshold( dstMask3, dstMask3, 0, 1, THRESH_BINARY );
bgModel_0P[j][1] = (float) gamma * thetaL;
} bitwise_and( dstMask2, dstMask3, dstMask );
} //copy correct B element of T1 inside T0 and swap
channelSplit[0][0].copyTo( tempMat );
channelSplit[1][0].copyTo( channelSplit[0][0], dstMask );
tempMat.copyTo( channelSplit[1][0], dstMask );
//copy correct C element of T0 inside T1
channelSplit[0][1].copyTo( channelSplit[1][1], dstMask );
//set new C0 values as gamma * thetaL
M_deltaL.mul( gamma );
M_deltaL.copyTo( channelSplit[0][1], dstMask );
for ( int i = 0; i < backGroundModelSize; i++ )
{
merge( channelSplit[i], *backgroundModel[i] );
} }
return true; return true;
} }
bool MotionSaliencyBinWangApr2014::templateReplacement( const Mat& finalBFMask, const Mat& image ) bool MotionSaliencyBinWangApr2014::templateReplacement( const Mat& finalBFMask, const Mat& image )
{ {
std::vector<Mat> temp; std::vector<Mat> temp;
...@@ -351,6 +396,8 @@ bool MotionSaliencyBinWangApr2014::templateReplacement( const Mat& finalBFMask, ...@@ -351,6 +396,8 @@ bool MotionSaliencyBinWangApr2014::templateReplacement( const Mat& finalBFMask,
{ {
thetaA = 50; thetaA = 50;
thetaL = 150; thetaL = 150;
/* thetaA = 5;
thetaL = 15;*/
neighborhoodCheck = false; neighborhoodCheck = false;
} }
...@@ -364,19 +411,19 @@ bool MotionSaliencyBinWangApr2014::templateReplacement( const Mat& finalBFMask, ...@@ -364,19 +411,19 @@ bool MotionSaliencyBinWangApr2014::templateReplacement( const Mat& finalBFMask,
int roiSize = 3; // FIXED ROI SIZE, not change until you first appropriately adjust the following controls in the EVALUATION section! int roiSize = 3; // FIXED ROI SIZE, not change until you first appropriately adjust the following controls in the EVALUATION section!
int countNonZeroElements = 0; int countNonZeroElements = 0;
std::vector<Mat> mv; std::vector<Mat> mv;
Mat replicateCurrentBAMat( roiSize, roiSize, CV_32F ); Mat replicateCurrentBAMat( roiSize, roiSize, CV_8U );
Mat backgroundModelROI( roiSize, roiSize, CV_32F ); Mat backgroundModelROI( roiSize, roiSize, CV_32F );
Mat diffResult( roiSize, roiSize, CV_32F ); Mat diffResult( roiSize, roiSize, CV_8U );
// Scan all pixels of finalBFMask and all pixels of others models (the dimension are the same) // Scan all pixels of finalBFMask and all pixels of others models (the dimension are the same)
const float* finalBFMaskP; const uchar* finalBFMaskP;
Vec2f* pbgP; Vec2b* pbgP;
const uchar* imageP; const uchar* imageP;
float* epslonP; float* epslonP;
for ( int i = 0; i < finalBFMask.rows; i++ ) for ( int i = 0; i < finalBFMask.rows; i++ )
{ {
finalBFMaskP = finalBFMask.ptr<float>( i ); finalBFMaskP = finalBFMask.ptr<uchar>( i );
pbgP = potentialBackground.ptr<Vec2f>( i ); pbgP = potentialBackground.ptr<Vec2b>( i );
imageP = image.ptr<uchar>( i ); imageP = image.ptr<uchar>( i );
epslonP = epslonPixelsValue.ptr<float>( i ); epslonP = epslonPixelsValue.ptr<float>( i );
for ( int j = 0; j < finalBFMask.cols; j++ ) for ( int j = 0; j < finalBFMask.cols; j++ )
...@@ -388,7 +435,7 @@ bool MotionSaliencyBinWangApr2014::templateReplacement( const Mat& finalBFMask, ...@@ -388,7 +435,7 @@ bool MotionSaliencyBinWangApr2014::templateReplacement( const Mat& finalBFMask,
* will be loaded into BA and CA will be set to 1*/ * will be loaded into BA and CA will be set to 1*/
if( pbgP[j][1] == 0 ) if( pbgP[j][1] == 0 )
{ {
pbgP[j][0] = (float) imageP[j]; pbgP[j][0] = imageP[j];
pbgP[j][1] = 1; pbgP[j][1] = 1;
} }
...@@ -471,6 +518,8 @@ bool MotionSaliencyBinWangApr2014::templateReplacement( const Mat& finalBFMask, ...@@ -471,6 +518,8 @@ bool MotionSaliencyBinWangApr2014::templateReplacement( const Mat& finalBFMask,
resize( replicateCurrentBAMat, replicateCurrentBAMat, Size( backgroundModelROI.cols, backgroundModelROI.rows ), 0, 0, INTER_LINEAR ); resize( replicateCurrentBAMat, replicateCurrentBAMat, Size( backgroundModelROI.cols, backgroundModelROI.rows ), 0, 0, INTER_LINEAR );
resize( diffResult, diffResult, Size( backgroundModelROI.cols, backgroundModelROI.rows ), 0, 0, INTER_LINEAR ); resize( diffResult, diffResult, Size( backgroundModelROI.cols, backgroundModelROI.rows ), 0, 0, INTER_LINEAR );
backgroundModelROI.convertTo( backgroundModelROI, CV_8U );
absdiff( replicateCurrentBAMat, backgroundModelROI, diffResult ); absdiff( replicateCurrentBAMat, backgroundModelROI, diffResult );
threshold( diffResult, diffResult, epslonP[j], 255, THRESH_BINARY_INV ); threshold( diffResult, diffResult, epslonP[j], 255, THRESH_BINARY_INV );
countNonZeroElements = countNonZero( diffResult ); countNonZeroElements = countNonZero( diffResult );
...@@ -479,9 +528,9 @@ bool MotionSaliencyBinWangApr2014::templateReplacement( const Mat& finalBFMask, ...@@ -479,9 +528,9 @@ bool MotionSaliencyBinWangApr2014::templateReplacement( const Mat& finalBFMask,
{ {
/////////////////// REPLACEMENT of backgroundModel template /////////////////// /////////////////// REPLACEMENT of backgroundModel template ///////////////////
//replace TA with current TK //replace TA with current TK
backgroundModel[backgroundModel.size() - 1]->at<Vec2f>( i, j ) = potentialBackground.at<Vec2f>( i, j ); backgroundModel[backgroundModel.size() - 1]->at<Vec2f>( i, j ) = potentialBackground.at<Vec2b>( i, j );
potentialBackground.at<Vec2f>( i, j )[0] = std::numeric_limits<float>::quiet_NaN(); potentialBackground.at<Vec2b>( i, j )[0] = 0;
potentialBackground.at<Vec2f>( i, j )[1] = 0; potentialBackground.at<Vec2b>( i, j )[1] = 0;
break; break;
} }
...@@ -489,9 +538,9 @@ bool MotionSaliencyBinWangApr2014::templateReplacement( const Mat& finalBFMask, ...@@ -489,9 +538,9 @@ bool MotionSaliencyBinWangApr2014::templateReplacement( const Mat& finalBFMask,
} }
else else
{ {
backgroundModel[backgroundModel.size() - 1]->at<Vec2f>( i, j ) = potentialBackground.at<Vec2f>( i, j ); backgroundModel[backgroundModel.size() - 1]->at<Vec2f>( i, j ) = potentialBackground.at<Vec2b>( i, j );
potentialBackground.at<Vec2f>( i, j )[0] = std::numeric_limits<float>::quiet_NaN(); potentialBackground.at<Vec2b>( i, j )[0] = 0;
potentialBackground.at<Vec2f>( i, j )[1] = 0; potentialBackground.at<Vec2b>( i, j )[1] = 0;
} }
} // close if of EVALUATION } // close if of EVALUATION
} // end of if( finalBFMask.at<uchar>( i, j ) == 1 ) // i.e. the corresponding frame pixel has been market as foreground } // end of if( finalBFMask.at<uchar>( i, j ) == 1 ) // i.e. the corresponding frame pixel has been market as foreground
...@@ -502,25 +551,108 @@ bool MotionSaliencyBinWangApr2014::templateReplacement( const Mat& finalBFMask, ...@@ -502,25 +551,108 @@ bool MotionSaliencyBinWangApr2014::templateReplacement( const Mat& finalBFMask,
return true; return true;
} }
bool MotionSaliencyBinWangApr2014::activityControl( const Mat& current_noisePixelsMask )
{
Mat discordanceFramesNoise, not_current_noisePixelsMask;
Mat nonZeroIndexes, not_discordanceFramesNoise; //u_current_noisePixelsMask;
//current_noisePixelsMask.convertTo( u_current_noisePixelsMask, CV_8UC1 );
// Derive the discrepancy between noise in the frame n-1 and frame n
//threshold( u_current_noisePixelsMask, not_current_noisePixelsMask, 0.5, 1.0, THRESH_BINARY_INV );
threshold( current_noisePixelsMask, not_current_noisePixelsMask, 0.5, 1.0, THRESH_BINARY_INV );
bitwise_and( noisePixelMask, not_current_noisePixelsMask, discordanceFramesNoise );
// indices in which the pixel at frame n-1 was the noise (or not) and now no (or yes) (blinking pixels)
findNonZero( discordanceFramesNoise, nonZeroIndexes );
Vec2i temp;
// we increase the activity value of these pixels
for ( int i = 0; i < nonZeroIndexes.rows; i++ )
{
//TODO check rows, cols inside at
temp = nonZeroIndexes.at<Vec2i>( i );
if( activityPixelsValue.at<uchar>( temp.val[1], temp.val[0] ) < Bmax )
{
activityPixelsValue.at<uchar>( temp.val[1], temp.val[0] ) += Ainc;
}
}
// decrement other pixels that have not changed (not blinking)
threshold( discordanceFramesNoise, not_discordanceFramesNoise, 0.5, 1.0, THRESH_BINARY_INV );
findNonZero( not_discordanceFramesNoise, nonZeroIndexes );
Vec2i temp2;
for ( int j = 0; j < nonZeroIndexes.rows; j++ )
{
temp2 = nonZeroIndexes.at<Vec2i>( j );
if( activityPixelsValue.at<uchar>( temp2.val[1], temp2.val[0] ) > 0 )
{
activityPixelsValue.at<uchar>( temp2.val[1], temp2.val[0] ) -= 1;
}
}
// update the noisePixelsMask
current_noisePixelsMask.copyTo( noisePixelMask );
return true;
}
bool MotionSaliencyBinWangApr2014::decisionThresholdAdaptation()
{
for ( int i = 0; i < activityPixelsValue.rows; i++ )
{
for ( int j = 0; j < activityPixelsValue.cols; j++ )
{
if( activityPixelsValue.at<uchar>( i, j ) > Binc && ( epslonPixelsValue.at<float>( i, j ) + deltaINC ) < epslonMAX )
{
epslonPixelsValue.at<float>( i, j ) += deltaINC;
}
else if( activityPixelsValue.at<uchar>( i, j ) < Bdec && ( epslonPixelsValue.at<float>( i, j ) - deltaDEC ) > epslonMIN )
{
epslonPixelsValue.at<float>( i, j ) -= deltaDEC;
}
}
}
return true;
}
bool MotionSaliencyBinWangApr2014::computeSaliencyImpl( InputArray image, OutputArray saliencyMap ) bool MotionSaliencyBinWangApr2014::computeSaliencyImpl( InputArray image, OutputArray saliencyMap )
{ {
Mat highResBFMask; CV_Assert(image.channels() == 1);
Mat lowResBFMask;
Mat highResBFMask, u_highResBFMask;
Mat lowResBFMask, u_lowResBFMask;
Mat not_lowResBFMask; Mat not_lowResBFMask;
Mat noisePixelsMask; Mat current_noisePixelsMask;
fullResolutionDetection( image.getMat(), highResBFMask ); fullResolutionDetection( image.getMat(), highResBFMask );
lowResolutionDetection( image.getMat(), lowResBFMask ); lowResolutionDetection( image.getMat(), lowResBFMask );
// Compute the final background-foreground mask. One pixel is marked as foreground if and only if it is // Compute the final background-foreground mask. One pixel is marked as foreground if and only if it is
// foreground in both masks (full and low) // foreground in both masks (full and low)
bitwise_and( highResBFMask, lowResBFMask, saliencyMap ); bitwise_and( highResBFMask, lowResBFMask, saliencyMap );
if( activityControlFlag )
{
// Detect the noise pixels (i.e. for a given pixel, fullRes(pixel) = foreground and lowRes(pixel)= background)
threshold( lowResBFMask, not_lowResBFMask, 0.5, 1.0, THRESH_BINARY_INV );
bitwise_and( highResBFMask, not_lowResBFMask, current_noisePixelsMask );
activityControl( current_noisePixelsMask );
decisionThresholdAdaptation();
}
templateOrdering(); templateOrdering();
templateReplacement( saliencyMap.getMat(), image.getMat() ); templateReplacement( saliencyMap.getMat(), image.getMat() );
templateOrdering(); templateOrdering();
activityControlFlag = true;
return true; return true;
} }
......
...@@ -51,19 +51,6 @@ Saliency::~Saliency() ...@@ -51,19 +51,6 @@ Saliency::~Saliency()
} }
Ptr<Saliency> Saliency::create( const String& saliencyType )
{
if (saliencyType == "SPECTRAL_RESIDUAL")
return makePtr<StaticSaliencySpectralResidual>();
else if (saliencyType == "FINE_GRAINED")
return makePtr<StaticSaliencyFineGrained>();
else if (saliencyType == "BING")
return makePtr<ObjectnessBING>();
else if (saliencyType == "BinWangApr2014")
return makePtr<MotionSaliencyBinWangApr2014>();
return Ptr<Saliency>();
}
bool Saliency::computeSaliency( InputArray image, OutputArray saliencyMap ) bool Saliency::computeSaliency( InputArray image, OutputArray saliencyMap )
{ {
if( image.empty() ) if( image.empty() )
...@@ -72,10 +59,5 @@ bool Saliency::computeSaliency( InputArray image, OutputArray saliencyMap ) ...@@ -72,10 +59,5 @@ bool Saliency::computeSaliency( InputArray image, OutputArray saliencyMap )
return computeSaliencyImpl( image, saliencyMap ); return computeSaliencyImpl( image, saliencyMap );
} }
String Saliency::getClassName() const
{
return className;
}
} /* namespace saliency */ } /* namespace saliency */
} /* namespace cv */ } /* namespace cv */
...@@ -61,7 +61,7 @@ namespace ppf_match_3d ...@@ -61,7 +61,7 @@ namespace ppf_match_3d
* and whether it should be loaded or not * and whether it should be loaded or not
* @return Returns the matrix on successfull load * @return Returns the matrix on successfull load
*/ */
CV_EXPORTS Mat loadPLYSimple(const char* fileName, int withNormals); CV_EXPORTS Mat loadPLYSimple(const char* fileName, int withNormals = 0);
/** /**
* @brief Write a point cloud to PLY file * @brief Write a point cloud to PLY file
......
...@@ -55,55 +55,92 @@ void getRandomRotation(double R[9]); ...@@ -55,55 +55,92 @@ void getRandomRotation(double R[9]);
void meanCovLocalPC(const float* pc, const int ws, const int point_count, double CovMat[3][3], double Mean[4]); void meanCovLocalPC(const float* pc, const int ws, const int point_count, double CovMat[3][3], double Mean[4]);
void meanCovLocalPCInd(const float* pc, const int* Indices, const int ws, const int point_count, double CovMat[3][3], double Mean[4]); void meanCovLocalPCInd(const float* pc, const int* Indices, const int ws, const int point_count, double CovMat[3][3], double Mean[4]);
static std::vector<std::string> split(const std::string &text, char sep) {
std::vector<std::string> tokens;
std::size_t start = 0, end = 0;
while ((end = text.find(sep, start)) != std::string::npos) {
tokens.push_back(text.substr(start, end - start));
start = end + 1;
}
tokens.push_back(text.substr(start));
return tokens;
}
Mat loadPLYSimple(const char* fileName, int withNormals) Mat loadPLYSimple(const char* fileName, int withNormals)
{ {
Mat cloud; Mat cloud;
int numVertices=0; int numVertices = 0;
int numCols = 3;
int has_normals = 0;
std::ifstream ifs(fileName); std::ifstream ifs(fileName);
if (!ifs.is_open()) if (!ifs.is_open())
{ CV_Error(Error::StsError, String("Error opening input file: ") + String(fileName) + "\n");
printf("Cannot open file...\n");
return Mat();
}
std::string str; std::string str;
while (str.substr(0, 10) !="end_header") while (str.substr(0, 10) != "end_header")
{
std::vector<std::string> tokens = split(str,' ');
if (tokens.size() == 3)
{
if (tokens[0] == "element" && tokens[1] == "vertex")
{
numVertices = atoi(tokens[2].c_str());
}
else if (tokens[0] == "property")
{
if (tokens[2] == "nx" || tokens[2] == "normal_x")
{
has_normals = -1;
numCols += 3;
}
else if (tokens[2] == "r" || tokens[2] == "red")
{ {
std::string entry = str.substr(0, 14); //has_color = true;
if (entry == "element vertex") numCols += 3;
}
else if (tokens[2] == "a" || tokens[2] == "alpha")
{ {
numVertices = atoi(str.substr(15, str.size()-15).c_str()); //has_alpha = true;
numCols += 1;
}
} }
}
else if (tokens.size() > 1 && tokens[0] == "format" && tokens[1] != "ascii")
CV_Error(Error::StsBadArg, String("Cannot read file, only ascii ply format is currently supported..."));
std::getline(ifs, str); std::getline(ifs, str);
} }
withNormals &= has_normals;
if (withNormals) cloud = Mat(numVertices, withNormals ? 6 : 3, CV_32FC1);
cloud=Mat(numVertices, 6, CV_32FC1);
else
cloud=Mat(numVertices, 3, CV_32FC1);
for (int i = 0; i < numVertices; i++) for (int i = 0; i < numVertices; i++)
{ {
float* data = cloud.ptr<float>(i); float* data = cloud.ptr<float>(i);
int col = 0;
for (; col < withNormals ? 6 : 3; ++col)
{
ifs >> data[col];
}
for (; col < numCols; ++col)
{
float tmp;
ifs >> tmp;
}
if (withNormals) if (withNormals)
{ {
ifs >> data[0] >> data[1] >> data[2] >> data[3] >> data[4] >> data[5];
// normalize to unit norm // normalize to unit norm
double norm = sqrt(data[3]*data[3] + data[4]*data[4] + data[5]*data[5]); double norm = sqrt(data[3]*data[3] + data[4]*data[4] + data[5]*data[5]);
if (norm>0.00001) if (norm>0.00001)
{ {
data[3]/=(float)norm; data[3]/=static_cast<float>(norm);
data[4]/=(float)norm; data[4]/=static_cast<float>(norm);
data[5]/=(float)norm; data[5]/=static_cast<float>(norm);
} }
} }
else
{
ifs >> data[0] >> data[1] >> data[2];
}
} }
//cloud *= 5.0f; //cloud *= 5.0f;
......
...@@ -538,8 +538,66 @@ at each window location. ...@@ -538,8 +538,66 @@ at each window location.
CV_EXPORTS_W Ptr<OCRBeamSearchDecoder::ClassifierCallback> loadOCRBeamSearchClassifierCNN(const String& filename); CV_EXPORTS_W Ptr<OCRBeamSearchDecoder::ClassifierCallback> loadOCRBeamSearchClassifierCNN(const String& filename);
/** @brief OCRHolisticWordRecognizer class provides the functionallity of segmented wordspotting.
* Given a predefined vocabulary , a DictNet is employed to select the most probable
* word given an input image.
*
* DictNet is described in detail in:
* Max Jaderberg et al.: Reading Text in the Wild with Convolutional Neural Networks, IJCV 2015
* http://arxiv.org/abs/1412.1842
*/
class CV_EXPORTS OCRHolisticWordRecognizer : public BaseOCR
{
public:
virtual void run(Mat& image,
std::string& output_text,
std::vector<Rect>* component_rects = NULL,
std::vector<std::string>* component_texts = NULL,
std::vector<float>* component_confidences = NULL,
int component_level = OCR_LEVEL_WORD) = 0;
/** @brief Recognize text using a segmentation based word-spotting/classifier cnn.
Takes image on input and returns recognized text in the output_text parameter. Optionally
provides also the Rects for individual text elements found (e.g. words), and the list of those
text elements with their confidence values.
@param image Input image CV_8UC1 or CV_8UC3
@param mask is totally ignored and is only available for compatibillity reasons
@param output_text Output text of the the word spoting, always one that exists in the dictionary.
@param component_rects Not applicable for word spotting can be be NULL if not, a single elemnt will
be put in the vector.
@param component_texts Not applicable for word spotting can be be NULL if not, a single elemnt will
be put in the vector.
@param component_confidences Not applicable for word spotting can be be NULL if not, a single elemnt will
be put in the vector.
@param component_level must be OCR_LEVEL_WORD.
*/
virtual void run(Mat& image,
Mat& mask,
std::string& output_text,
std::vector<Rect>* component_rects = NULL,
std::vector<std::string>* component_texts = NULL,
std::vector<float>* component_confidences = NULL,
int component_level = OCR_LEVEL_WORD) = 0;
/** @brief Creates an instance of the OCRHolisticWordRecognizer class.
*/
static Ptr<OCRHolisticWordRecognizer> create(const std::string &archFilename,
const std::string &weightsFilename,
const std::string &wordsFilename);
};
//! @} //! @}
} }} // cv::text::
}
#endif // _OPENCV_TEXT_OCR_HPP_ #endif // _OPENCV_TEXT_OCR_HPP_
/*
* dictnet_demo.cpp
*
* Demonstrates simple use of the holistic word classifier in C++
*
* Created on: June 26, 2016
* Author: Anguelos Nicolaou <anguelos.nicolaou AT gmail.com>
*/
#include "opencv2/text.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/imgproc.hpp"
#include <sstream>
#include <iostream>
using namespace std;
using namespace cv;
using namespace cv::text;
inline void printHelp()
{
cout << " Demo of wordspotting CNN for text recognition." << endl;
cout << " Max Jaderberg et al.: Reading Text in the Wild with Convolutional Neural Networks, IJCV 2015"<<std::endl<<std::endl;
cout << " Usage: program <input_image>" << endl;
cout << " Caffe Model files (dictnet_vgg.caffemodel, dictnet_vgg_deploy.prototxt, dictnet_vgg_labels.txt)"<<endl;
cout << " must be in the current directory." << endl << endl;
cout << " Obtaining Caffe Model files in linux shell:"<<endl;
cout << " wget http://nicolaou.homouniversalis.org/assets/vgg_text/dictnet_vgg.caffemodel"<<endl;
cout << " wget http://nicolaou.homouniversalis.org/assets/vgg_text/dictnet_vgg_deploy.prototxt"<<endl;
cout << " wget http://nicolaou.homouniversalis.org/assets/vgg_text/dictnet_vgg_labels.txt"<<endl<<endl;
}
int main(int argc, const char * argv[])
{
if (argc != 2)
{
printHelp();
exit(1);
}
Mat image = imread(argv[1], IMREAD_GRAYSCALE);
cout << "Read image (" << argv[1] << "): " << image.size << ", channels: " << image.channels() << ", depth: " << image.depth() << endl;
if (image.empty())
{
printHelp();
exit(1);
}
Ptr<OCRHolisticWordRecognizer> wordSpotter = OCRHolisticWordRecognizer::create("dictnet_vgg_deploy.prototxt", "dictnet_vgg.caffemodel", "dictnet_vgg_labels.txt");
std::string word;
vector<float> confs;
wordSpotter->run(image, word, 0, 0, &confs);
cout << "Detected word: '" << word << "', confidence: " << confs[0] << endl;
}
#include "precomp.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/core.hpp"
#include "opencv2/dnn.hpp"
#include <fstream>
using namespace std;
namespace cv { namespace text {
class OCRHolisticWordRecognizerImpl : public OCRHolisticWordRecognizer
{
private:
dnn::Net net;
vector<string> words;
public:
OCRHolisticWordRecognizerImpl(const string &archFilename, const string &weightsFilename, const string &wordsFilename)
{
net = dnn::readNetFromCaffe(archFilename, weightsFilename);
std::ifstream in(wordsFilename.c_str());
if (!in)
{
CV_Error(Error::StsError, "Could not read Labels from file");
}
std::string line;
while (std::getline(in, line))
words.push_back(line);
CV_Assert(getClassCount() == words.size());
}
void run(Mat& image, std::string& output_text, std::vector<Rect>* component_rects=NULL, std::vector<std::string>* component_texts=NULL, std::vector<float>* component_confidences=NULL, int component_level=0)
{
CV_Assert(component_level==OCR_LEVEL_WORD); //Componnents not applicable for word spotting
double confidence;
output_text = classify(image, confidence);
if(component_rects!=NULL){
component_rects->resize(1);
(*component_rects)[0]=Rect(0,0,image.size().width,image.size().height);
}
if(component_texts!=NULL){
component_texts->resize(1);
(*component_texts)[0] = output_text;
}
if(component_confidences!=NULL){
component_confidences->resize(1);
(*component_confidences)[0] = float(confidence);
}
}
void run(Mat& image, Mat& mask, std::string& output_text, std::vector<Rect>* component_rects=NULL, std::vector<std::string>* component_texts=NULL, std::vector<float>* component_confidences=NULL, int component_level=0)
{
//Mask is ignored because the CNN operates on a full image
CV_Assert(mask.cols == image.cols && mask.rows == image.rows);
this->run(image, output_text, component_rects, component_texts, component_confidences, component_level);
}
protected:
Size getPerceptiveField() const
{
return Size(100, 32);
}
size_t getClassCount()
{
int id = net.getLayerId("prob");
dnn::MatShape inputShape;
inputShape.push_back(1);
inputShape.push_back(1);
inputShape.push_back(getPerceptiveField().height);
inputShape.push_back(getPerceptiveField().width);
vector<dnn::MatShape> inShapes, outShapes;
net.getLayerShapes(inputShape, id, inShapes, outShapes);
CV_Assert(outShapes.size() == 1 && outShapes[0].size() == 4);
CV_Assert(outShapes[0][0] == 1 && outShapes[0][2] == 1 && outShapes[0][3] == 1);
return outShapes[0][1];
}
string classify(InputArray image, double & conf)
{
CV_Assert(image.channels() == 1 && image.depth() == CV_8U);
Mat resized;
resize(image, resized, getPerceptiveField());
Mat blob = dnn::blobFromImage(resized);
net.setInput(blob, "data");
Mat prob = net.forward("prob");
CV_Assert(prob.dims == 4 && !prob.empty() && prob.size[1] == (int)getClassCount());
int idx[4] = {0};
minMaxIdx(prob, 0, &conf, 0, idx);
CV_Assert(0 <= idx[1] && idx[1] < (int)words.size());
return words[idx[1]];
}
};
Ptr<OCRHolisticWordRecognizer> OCRHolisticWordRecognizer::create(const string &archFilename, const string &weightsFilename, const string &wordsFilename)
{
return makePtr<OCRHolisticWordRecognizerImpl>(archFilename, weightsFilename, wordsFilename);
}
}} // cv::text::
...@@ -1236,12 +1236,12 @@ public: ...@@ -1236,12 +1236,12 @@ public:
*/ */
void write(FileStorage& /*fs*/) const; void write(FileStorage& /*fs*/) const;
double detect_thresh; //!< detection confidence threshold float detect_thresh; //!< detection confidence threshold
double sigma; //!< gaussian kernel bandwidth float sigma; //!< gaussian kernel bandwidth
double lambda; //!< regularization float lambda; //!< regularization
double interp_factor; //!< linear interpolation factor for adaptation float interp_factor; //!< linear interpolation factor for adaptation
double output_sigma_factor; //!< spatial bandwidth (proportional to target) float output_sigma_factor; //!< spatial bandwidth (proportional to target)
double pca_learning_rate; //!< compression learning rate float pca_learning_rate; //!< compression learning rate
bool resize; //!< activate the resize feature to improve the processing speed bool resize; //!< activate the resize feature to improve the processing speed
bool split_coeff; //!< split the training coefficients into two matrices bool split_coeff; //!< split the training coefficients into two matrices
bool wrap_kernel; //!< wrap around the kernel values bool wrap_kernel; //!< wrap around the kernel values
......
...@@ -117,6 +117,7 @@ int main( int argc, char** argv ){ ...@@ -117,6 +117,7 @@ int main( int argc, char** argv ){
bool initialized = false; bool initialized = false;
int frameCounter = 0; int frameCounter = 0;
int64 timeTotal = 0;
for ( ;; ) for ( ;; )
{ {
...@@ -142,11 +143,14 @@ int main( int argc, char** argv ){ ...@@ -142,11 +143,14 @@ int main( int argc, char** argv ){
} }
else if( initialized ) else if( initialized )
{ {
int64 frameTime = getTickCount();
//updates the tracker //updates the tracker
if( tracker->update( frame, boundingBox ) ) if( tracker->update( frame, boundingBox ) )
{ {
rectangle( image, boundingBox, Scalar( 255, 0, 0 ), 2, 1 ); rectangle( image, boundingBox, Scalar( 255, 0, 0 ), 2, 1 );
} }
frameTime = getTickCount() - frameTime;
timeTotal += frameTime;
} }
imshow( "Tracking API", image ); imshow( "Tracking API", image );
frameCounter++; frameCounter++;
...@@ -159,5 +163,8 @@ int main( int argc, char** argv ){ ...@@ -159,5 +163,8 @@ int main( int argc, char** argv ){
paused = !paused; paused = !paused;
} }
double s = frameCounter / (timeTotal / getTickFrequency());
printf("FPS: %f\n", s);
return 0; return 0;
} }
This source diff could not be displayed because it is too large. You can view the blob instead.
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.
// Copyright (C) 2016, Intel, Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
#define LOCAL_SIZE_X 64
#define BLOCK_SIZE_X 3
__kernel void tmm(__global float *A, int m, int n, float alpha, __global float *D)
{
int lidX = get_local_id(0);
uint lsizeX = get_local_size(0);
uint matI = get_group_id(1);
uint matJ = get_group_id(0);
if (matI < matJ)
return;
__local float4 a[LOCAL_SIZE_X], b[LOCAL_SIZE_X];
float4 result;
__local uint cnt;
result = 0;
cnt = 0;
barrier(CLK_LOCAL_MEM_FENCE);
do {
// load block data to SLM.
int global_block_base = (lidX + cnt * lsizeX) * BLOCK_SIZE_X;
float4 pa[BLOCK_SIZE_X], pb[BLOCK_SIZE_X];
#pragma unroll
for(uint j = 0; j < BLOCK_SIZE_X && (cnt * lsizeX + lidX) * BLOCK_SIZE_X < n / 4; j++) {
pa[j] = *(__global float4*)&A[matI * n + (global_block_base + j) * 4];
if (matI != matJ)
pb[j] = *(__global float4*)&A[matJ * n + (global_block_base + j) * 4];
else
pb[j] = pa[j];
}
// zero the data out-of-boundary.
if (global_block_base + BLOCK_SIZE_X - 1 >= n/4) {
#pragma unroll
for(int i = 0; i < BLOCK_SIZE_X; i++) {
if (global_block_base + i >= n/4)
pb[i] = 0;
}
}
pb[0] *= pa[0];
for(int j = 1; j < BLOCK_SIZE_X; j++)
pb[0] = fma(pb[j], pa[j], pb[0]);
b[lidX] = pb[0];
barrier(CLK_LOCAL_MEM_FENCE);
// perform reduce add
for(int offset = LOCAL_SIZE_X / 2; offset > 0; offset >>= 1) {
if (lidX < offset)
b[lidX] += b[(lidX + offset)];
barrier(CLK_LOCAL_MEM_FENCE);
}
if (lidX == 0) {
result += b[0];
cnt++;
}
barrier(CLK_LOCAL_MEM_FENCE);
} while(cnt * BLOCK_SIZE_X * lsizeX < n / 4);
if (lidX == 0) {
float ret = (result.s0 + result.s1 + result.s2 + result.s3) * alpha;
D[matI * m + matJ] = ret;
if (matI != matJ)
D[matJ * m + matI] = ret;
}
}
...@@ -42,6 +42,7 @@ ...@@ -42,6 +42,7 @@
#ifndef __OPENCV_PRECOMP_H__ #ifndef __OPENCV_PRECOMP_H__
#define __OPENCV_PRECOMP_H__ #define __OPENCV_PRECOMP_H__
#include "cvconfig.h"
#include "opencv2/tracking.hpp" #include "opencv2/tracking.hpp"
#include "opencv2/core/utility.hpp" #include "opencv2/core/utility.hpp"
#include "opencv2/core/ocl.hpp" #include "opencv2/core/ocl.hpp"
...@@ -50,7 +51,7 @@ ...@@ -50,7 +51,7 @@
namespace cv namespace cv
{ {
extern const double ColorNames[][10]; extern const float ColorNames[][10];
namespace tracking { namespace tracking {
......
...@@ -40,7 +40,9 @@ ...@@ -40,7 +40,9 @@
//M*/ //M*/
#include "precomp.hpp" #include "precomp.hpp"
#include "opencl_kernels_tracking.hpp"
#include <complex> #include <complex>
#include <cmath>
/*--------------------------- /*---------------------------
| TrackerKCFModel | TrackerKCFModel
...@@ -93,13 +95,13 @@ namespace cv{ ...@@ -93,13 +95,13 @@ namespace cv{
void inline ifft2(const Mat src, Mat & dest) const; void inline ifft2(const Mat src, Mat & dest) const;
void inline pixelWiseMult(const std::vector<Mat> src1, const std::vector<Mat> src2, std::vector<Mat> & dest, const int flags, const bool conjB=false) const; void inline pixelWiseMult(const std::vector<Mat> src1, const std::vector<Mat> src2, std::vector<Mat> & dest, const int flags, const bool conjB=false) const;
void inline sumChannels(std::vector<Mat> src, Mat & dest) const; void inline sumChannels(std::vector<Mat> src, Mat & dest) const;
void inline updateProjectionMatrix(const Mat src, Mat & old_cov,Mat & proj_matrix,double pca_rate, int compressed_sz, void inline updateProjectionMatrix(const Mat src, Mat & old_cov,Mat & proj_matrix,float pca_rate, int compressed_sz,
std::vector<Mat> & layers_pca,std::vector<Scalar> & average, Mat pca_data, Mat new_cov, Mat w, Mat u, Mat v) const; std::vector<Mat> & layers_pca,std::vector<Scalar> & average, Mat pca_data, Mat new_cov, Mat w, Mat u, Mat v);
void inline compress(const Mat proj_matrix, const Mat src, Mat & dest, Mat & data, Mat & compressed) const; void inline compress(const Mat proj_matrix, const Mat src, Mat & dest, Mat & data, Mat & compressed) const;
bool getSubWindow(const Mat img, const Rect roi, Mat& feat, Mat& patch, TrackerKCF::MODE desc = GRAY) const; bool getSubWindow(const Mat img, const Rect roi, Mat& feat, Mat& patch, TrackerKCF::MODE desc = GRAY) const;
bool getSubWindow(const Mat img, const Rect roi, Mat& feat, void (*f)(const Mat, const Rect, Mat& )) const; bool getSubWindow(const Mat img, const Rect roi, Mat& feat, void (*f)(const Mat, const Rect, Mat& )) const;
void extractCN(Mat patch_data, Mat & cnFeatures) const; void extractCN(Mat patch_data, Mat & cnFeatures) const;
void denseGaussKernel(const double sigma, const Mat , const Mat y_data, Mat & k_data, void denseGaussKernel(const float sigma, const Mat , const Mat y_data, Mat & k_data,
std::vector<Mat> & layers_data,std::vector<Mat> & xf_data,std::vector<Mat> & yf_data, std::vector<Mat> xyf_v, Mat xy, Mat xyf ) const; std::vector<Mat> & layers_data,std::vector<Mat> & xf_data,std::vector<Mat> & yf_data, std::vector<Mat> xyf_v, Mat xy, Mat xyf ) const;
void calcResponse(const Mat alphaf_data, const Mat kf_data, Mat & response_data, Mat & spec_data) const; void calcResponse(const Mat alphaf_data, const Mat kf_data, Mat & response_data, Mat & spec_data) const;
void calcResponse(const Mat alphaf_data, const Mat alphaf_den_data, const Mat kf_data, Mat & response_data, Mat & spec_data, Mat & spec2_data) const; void calcResponse(const Mat alphaf_data, const Mat alphaf_den_data, const Mat kf_data, Mat & response_data, Mat & spec_data, Mat & spec2_data) const;
...@@ -107,9 +109,12 @@ namespace cv{ ...@@ -107,9 +109,12 @@ namespace cv{
void shiftRows(Mat& mat) const; void shiftRows(Mat& mat) const;
void shiftRows(Mat& mat, int n) const; void shiftRows(Mat& mat, int n) const;
void shiftCols(Mat& mat, int n) const; void shiftCols(Mat& mat, int n) const;
#ifdef HAVE_OPENCL
bool inline oclTransposeMM(const Mat src, float alpha, UMat &dst);
#endif
private: private:
double output_sigma; float output_sigma;
Rect2d roi; Rect2d roi;
Mat hann; //hann window filter Mat hann; //hann window filter
Mat hann_cn; //10 dimensional hann-window filter for CN features, Mat hann_cn; //10 dimensional hann-window filter for CN features,
...@@ -154,6 +159,10 @@ namespace cv{ ...@@ -154,6 +159,10 @@ namespace cv{
bool resizeImage; // resize the image whenever needed and the patch size is large bool resizeImage; // resize the image whenever needed and the patch size is large
#ifdef HAVE_OPENCL
ocl::Kernel transpose_mm_ker; // OCL kernel to compute transpose matrix multiply matrix.
#endif
int frame; int frame;
}; };
...@@ -174,6 +183,16 @@ namespace cv{ ...@@ -174,6 +183,16 @@ namespace cv{
use_custom_extractor_pca = false; use_custom_extractor_pca = false;
use_custom_extractor_npca = false; use_custom_extractor_npca = false;
#ifdef HAVE_OPENCL
// For update proj matrix's multiplication
if(ocl::useOpenCL())
{
cv::String err;
ocl::ProgramSource tmmSrc = ocl::tracking::tmm_oclsrc;
ocl::Program tmmProg(tmmSrc, String(), err);
transpose_mm_ker.create("tmm", tmmProg);
}
#endif
} }
void TrackerKCFImpl::read( const cv::FileNode& fn ){ void TrackerKCFImpl::read( const cv::FileNode& fn ){
...@@ -196,8 +215,8 @@ namespace cv{ ...@@ -196,8 +215,8 @@ namespace cv{
roi = boundingBox; roi = boundingBox;
//calclulate output sigma //calclulate output sigma
output_sigma=sqrt(roi.width*roi.height)*params.output_sigma_factor; output_sigma=std::sqrt(static_cast<float>(roi.width*roi.height))*params.output_sigma_factor;
output_sigma=-0.5/(output_sigma*output_sigma); output_sigma=-0.5f/(output_sigma*output_sigma);
//resize the ROI whenever needed //resize the ROI whenever needed
if(params.resize && roi.width*roi.height>params.max_patch_size){ if(params.resize && roi.width*roi.height>params.max_patch_size){
...@@ -215,21 +234,22 @@ namespace cv{ ...@@ -215,21 +234,22 @@ namespace cv{
roi.height*=2; roi.height*=2;
// initialize the hann window filter // initialize the hann window filter
createHanningWindow(hann, roi.size(), CV_64F); createHanningWindow(hann, roi.size(), CV_32F);
// hann window filter for CN feature // hann window filter for CN feature
Mat _layer[] = {hann, hann, hann, hann, hann, hann, hann, hann, hann, hann}; Mat _layer[] = {hann, hann, hann, hann, hann, hann, hann, hann, hann, hann};
merge(_layer, 10, hann_cn); merge(_layer, 10, hann_cn);
// create gaussian response // create gaussian response
y=Mat::zeros((int)roi.height,(int)roi.width,CV_64F); y=Mat::zeros((int)roi.height,(int)roi.width,CV_32F);
for(unsigned i=0;i<(unsigned)roi.height;i++){ for(int i=0;i<roi.height;i++){
for(unsigned j=0;j<(unsigned)roi.width;j++){ for(int j=0;j<roi.width;j++){
y.at<double>(i,j)=(i-roi.height/2+1)*(i-roi.height/2+1)+(j-roi.width/2+1)*(j-roi.width/2+1); y.at<float>(i,j) =
static_cast<float>((i-roi.height/2+1)*(i-roi.height/2+1)+(j-roi.width/2+1)*(j-roi.width/2+1));
} }
} }
y*=(double)output_sigma; y*=(float)output_sigma;
cv::exp(y,y); cv::exp(y,y);
// perform fourier transfor to the gaussian response // perform fourier transfor to the gaussian response
...@@ -331,7 +351,7 @@ namespace cv{ ...@@ -331,7 +351,7 @@ namespace cv{
// compute the fourier transform of the kernel // compute the fourier transform of the kernel
fft2(k,kf); fft2(k,kf);
if(frame==1)spec2=Mat_<Vec2d >(kf.rows, kf.cols); if(frame==1)spec2=Mat_<Vec2f >(kf.rows, kf.cols);
// calculate filter response // calculate filter response
if(params.split_coeff) if(params.split_coeff)
...@@ -411,7 +431,7 @@ namespace cv{ ...@@ -411,7 +431,7 @@ namespace cv{
vxf.resize(x.channels()); vxf.resize(x.channels());
vyf.resize(x.channels()); vyf.resize(x.channels());
vxyf.resize(vyf.size()); vxyf.resize(vyf.size());
new_alphaf=Mat_<Vec2d >(yf.rows, yf.cols); new_alphaf=Mat_<Vec2f >(yf.rows, yf.cols);
} }
// Kernel Regularized Least-Squares, calculate alphas // Kernel Regularized Least-Squares, calculate alphas
...@@ -421,19 +441,19 @@ namespace cv{ ...@@ -421,19 +441,19 @@ namespace cv{
fft2(k,kf); fft2(k,kf);
kf_lambda=kf+params.lambda; kf_lambda=kf+params.lambda;
double den; float den;
if(params.split_coeff){ if(params.split_coeff){
mulSpectrums(yf,kf,new_alphaf,0); mulSpectrums(yf,kf,new_alphaf,0);
mulSpectrums(kf,kf_lambda,new_alphaf_den,0); mulSpectrums(kf,kf_lambda,new_alphaf_den,0);
}else{ }else{
for(int i=0;i<yf.rows;i++){ for(int i=0;i<yf.rows;i++){
for(int j=0;j<yf.cols;j++){ for(int j=0;j<yf.cols;j++){
den = 1.0/(kf_lambda.at<Vec2d>(i,j)[0]*kf_lambda.at<Vec2d>(i,j)[0]+kf_lambda.at<Vec2d>(i,j)[1]*kf_lambda.at<Vec2d>(i,j)[1]); den = 1.0f/(kf_lambda.at<Vec2f>(i,j)[0]*kf_lambda.at<Vec2f>(i,j)[0]+kf_lambda.at<Vec2f>(i,j)[1]*kf_lambda.at<Vec2f>(i,j)[1]);
new_alphaf.at<Vec2d>(i,j)[0]= new_alphaf.at<Vec2f>(i,j)[0]=
(yf.at<Vec2d>(i,j)[0]*kf_lambda.at<Vec2d>(i,j)[0]+yf.at<Vec2d>(i,j)[1]*kf_lambda.at<Vec2d>(i,j)[1])*den; (yf.at<Vec2f>(i,j)[0]*kf_lambda.at<Vec2f>(i,j)[0]+yf.at<Vec2f>(i,j)[1]*kf_lambda.at<Vec2f>(i,j)[1])*den;
new_alphaf.at<Vec2d>(i,j)[1]= new_alphaf.at<Vec2f>(i,j)[1]=
(yf.at<Vec2d>(i,j)[1]*kf_lambda.at<Vec2d>(i,j)[0]-yf.at<Vec2d>(i,j)[0]*kf_lambda.at<Vec2d>(i,j)[1])*den; (yf.at<Vec2f>(i,j)[1]*kf_lambda.at<Vec2f>(i,j)[0]-yf.at<Vec2f>(i,j)[0]*kf_lambda.at<Vec2f>(i,j)[1])*den;
} }
} }
} }
...@@ -467,24 +487,25 @@ namespace cv{ ...@@ -467,24 +487,25 @@ namespace cv{
int rows = dst.rows, cols = dst.cols; int rows = dst.rows, cols = dst.cols;
AutoBuffer<double> _wc(cols); AutoBuffer<float> _wc(cols);
double * const wc = (double *)_wc; float * const wc = (float *)_wc;
double coeff0 = 2.0 * CV_PI / (double)(cols - 1), coeff1 = 2.0f * CV_PI / (double)(rows - 1); const float coeff0 = 2.0f * (float)CV_PI / (cols - 1);
const float coeff1 = 2.0f * (float)CV_PI / (rows - 1);
for(int j = 0; j < cols; j++) for(int j = 0; j < cols; j++)
wc[j] = 0.5 * (1.0 - cos(coeff0 * j)); wc[j] = 0.5f * (1.0f - cos(coeff0 * j));
if(dst.depth() == CV_32F){ if(dst.depth() == CV_32F){
for(int i = 0; i < rows; i++){ for(int i = 0; i < rows; i++){
float* dstData = dst.ptr<float>(i); float* dstData = dst.ptr<float>(i);
double wr = 0.5 * (1.0 - cos(coeff1 * i)); float wr = 0.5f * (1.0f - cos(coeff1 * i));
for(int j = 0; j < cols; j++) for(int j = 0; j < cols; j++)
dstData[j] = (float)(wr * wc[j]); dstData[j] = (float)(wr * wc[j]);
} }
}else{ }else{
for(int i = 0; i < rows; i++){ for(int i = 0; i < rows; i++){
double* dstData = dst.ptr<double>(i); double* dstData = dst.ptr<double>(i);
double wr = 0.5 * (1.0 - cos(coeff1 * i)); double wr = 0.5f * (1.0f - cos(coeff1 * i));
for(int j = 0; j < cols; j++) for(int j = 0; j < cols; j++)
dstData[j] = wr * wc[j]; dstData[j] = wr * wc[j];
} }
...@@ -535,11 +556,37 @@ namespace cv{ ...@@ -535,11 +556,37 @@ namespace cv{
} }
} }
#ifdef HAVE_OPENCL
bool inline TrackerKCFImpl::oclTransposeMM(const Mat src, float alpha, UMat &dst){
// Current kernel only support matrix's rows is multiple of 4.
// And if one line is less than 512KB, CPU will likely be faster.
if (transpose_mm_ker.empty() ||
src.rows % 4 != 0 ||
(src.rows * 10) < (1024 * 1024 / 4))
return false;
Size s(src.rows, src.cols);
const Mat tmp = src.t();
const UMat uSrc = tmp.getUMat(ACCESS_READ);
transpose_mm_ker.args(
ocl::KernelArg::PtrReadOnly(uSrc),
(int)uSrc.rows,
(int)uSrc.cols,
alpha,
ocl::KernelArg::PtrWriteOnly(dst));
size_t globSize[2] = {static_cast<size_t>(src.cols * 64), static_cast<size_t>(src.cols)};
size_t localSize[2] = {64, 1};
if (!transpose_mm_ker.run(2, globSize, localSize, true))
return false;
return true;
}
#endif
/* /*
* obtains the projection matrix using PCA * obtains the projection matrix using PCA
*/ */
void inline TrackerKCFImpl::updateProjectionMatrix(const Mat src, Mat & old_cov,Mat & proj_matrix, double pca_rate, int compressed_sz, void inline TrackerKCFImpl::updateProjectionMatrix(const Mat src, Mat & old_cov,Mat & proj_matrix, float pca_rate, int compressed_sz,
std::vector<Mat> & layers_pca,std::vector<Scalar> & average, Mat pca_data, Mat new_cov, Mat w, Mat u, Mat vt) const { std::vector<Mat> & layers_pca,std::vector<Scalar> & average, Mat pca_data, Mat new_cov, Mat w, Mat u, Mat vt) {
CV_Assert(compressed_sz<=src.channels()); CV_Assert(compressed_sz<=src.channels());
split(src,layers_pca); split(src,layers_pca);
...@@ -553,17 +600,40 @@ namespace cv{ ...@@ -553,17 +600,40 @@ namespace cv{
merge(layers_pca,pca_data); merge(layers_pca,pca_data);
pca_data=pca_data.reshape(1,src.rows*src.cols); pca_data=pca_data.reshape(1,src.rows*src.cols);
new_cov=1.0/(double)(src.rows*src.cols-1)*(pca_data.t()*pca_data); #ifdef HAVE_OPENCL
bool oclSucceed = false;
Size s(pca_data.cols, pca_data.cols);
UMat result(s, pca_data.type());
if (oclTransposeMM(pca_data, 1.0f/(float)(src.rows*src.cols-1), result)) {
if(old_cov.rows==0) old_cov=result.getMat(ACCESS_READ).clone();
SVD::compute((1.0-pca_rate)*old_cov + pca_rate * result.getMat(ACCESS_READ), w, u, vt);
oclSucceed = true;
}
#define TMM_VERIFICATION 0
if (oclSucceed == false || TMM_VERIFICATION) {
new_cov=1.0f/(float)(src.rows*src.cols-1)*(pca_data.t()*pca_data);
#if TMM_VERIFICATION
for(int i = 0; i < new_cov.rows; i++)
for(int j = 0; j < new_cov.cols; j++)
if (abs(new_cov.at<float>(i, j) - result.getMat(ACCESS_RW).at<float>(i , j)) > abs(new_cov.at<float>(i, j)) * 1e-3)
printf("error @ i %d j %d got %G expected %G \n", i, j, result.getMat(ACCESS_RW).at<float>(i , j), new_cov.at<float>(i, j));
#endif
if(old_cov.rows==0)old_cov=new_cov.clone();
SVD::compute((1.0f - pca_rate) * old_cov + pca_rate * new_cov, w, u, vt);
}
#else
new_cov=1.0/(float)(src.rows*src.cols-1)*(pca_data.t()*pca_data);
if(old_cov.rows==0)old_cov=new_cov.clone(); if(old_cov.rows==0)old_cov=new_cov.clone();
// calc PCA // calc PCA
SVD::compute((1.0-pca_rate)*old_cov+pca_rate*new_cov, w, u, vt); SVD::compute((1.0-pca_rate)*old_cov+pca_rate*new_cov, w, u, vt);
#endif
// extract the projection matrix // extract the projection matrix
proj_matrix=u(Rect(0,0,compressed_sz,src.channels())).clone(); proj_matrix=u(Rect(0,0,compressed_sz,src.channels())).clone();
Mat proj_vars=Mat::eye(compressed_sz,compressed_sz,proj_matrix.type()); Mat proj_vars=Mat::eye(compressed_sz,compressed_sz,proj_matrix.type());
for(int i=0;i<compressed_sz;i++){ for(int i=0;i<compressed_sz;i++){
proj_vars.at<double>(i,i)=w.at<double>(i); proj_vars.at<float>(i,i)=w.at<float>(i);
} }
// update the covariance matrix // update the covariance matrix
...@@ -622,8 +692,9 @@ namespace cv{ ...@@ -622,8 +692,9 @@ namespace cv{
cvtColor(patch,feat, CV_BGR2GRAY); cvtColor(patch,feat, CV_BGR2GRAY);
else else
feat=patch; feat=patch;
feat.convertTo(feat,CV_64F); //feat.convertTo(feat,CV_32F);
feat=feat/255.0-0.5; // normalize to range -0.5 .. 0.5 feat.convertTo(feat,CV_32F, 1.0/255.0, -0.5);
//feat=feat/255.0-0.5; // normalize to range -0.5 .. 0.5
feat=feat.mul(hann); // hann window filter feat=feat.mul(hann); // hann window filter
break; break;
} }
...@@ -670,8 +741,8 @@ namespace cv{ ...@@ -670,8 +741,8 @@ namespace cv{
Vec3b & pixel = patch_data.at<Vec3b>(0,0); Vec3b & pixel = patch_data.at<Vec3b>(0,0);
unsigned index; unsigned index;
if(cnFeatures.type() != CV_64FC(10)) if(cnFeatures.type() != CV_32FC(10))
cnFeatures = Mat::zeros(patch_data.rows,patch_data.cols,CV_64FC(10)); cnFeatures = Mat::zeros(patch_data.rows,patch_data.cols,CV_32FC(10));
for(int i=0;i<patch_data.rows;i++){ for(int i=0;i<patch_data.rows;i++){
for(int j=0;j<patch_data.cols;j++){ for(int j=0;j<patch_data.cols;j++){
...@@ -680,7 +751,7 @@ namespace cv{ ...@@ -680,7 +751,7 @@ namespace cv{
//copy the values //copy the values
for(int _k=0;_k<10;_k++){ for(int _k=0;_k<10;_k++){
cnFeatures.at<Vec<double,10> >(i,j)[_k]=ColorNames[index][_k]; cnFeatures.at<Vec<float,10> >(i,j)[_k]=ColorNames[index][_k];
} }
} }
} }
...@@ -690,7 +761,7 @@ namespace cv{ ...@@ -690,7 +761,7 @@ namespace cv{
/* /*
* dense gauss kernel function * dense gauss kernel function
*/ */
void TrackerKCFImpl::denseGaussKernel(const double sigma, const Mat x_data, const Mat y_data, Mat & k_data, void TrackerKCFImpl::denseGaussKernel(const float sigma, const Mat x_data, const Mat y_data, Mat & k_data,
std::vector<Mat> & layers_data,std::vector<Mat> & xf_data,std::vector<Mat> & yf_data, std::vector<Mat> xyf_v, Mat xy, Mat xyf ) const { std::vector<Mat> & layers_data,std::vector<Mat> & xf_data,std::vector<Mat> & yf_data, std::vector<Mat> xyf_v, Mat xy, Mat xyf ) const {
double normX, normY; double normX, normY;
...@@ -718,11 +789,11 @@ namespace cv{ ...@@ -718,11 +789,11 @@ namespace cv{
//threshold(xy,xy,0.0,0.0,THRESH_TOZERO);//max(0, (xx + yy - 2 * xy) / numel(x)) //threshold(xy,xy,0.0,0.0,THRESH_TOZERO);//max(0, (xx + yy - 2 * xy) / numel(x))
for(int i=0;i<xy.rows;i++){ for(int i=0;i<xy.rows;i++){
for(int j=0;j<xy.cols;j++){ for(int j=0;j<xy.cols;j++){
if(xy.at<double>(i,j)<0.0)xy.at<double>(i,j)=0.0; if(xy.at<float>(i,j)<0.0)xy.at<float>(i,j)=0.0;
} }
} }
double sig=-1.0/(sigma*sigma); float sig=-1.0f/(sigma*sigma);
xy=sig*xy; xy=sig*xy;
exp(xy,k_data); exp(xy,k_data);
...@@ -796,14 +867,14 @@ namespace cv{ ...@@ -796,14 +867,14 @@ namespace cv{
mulSpectrums(alphaf_data,kf_data,spec_data,0,false); mulSpectrums(alphaf_data,kf_data,spec_data,0,false);
//z=(a+bi)/(c+di)=[(ac+bd)+i(bc-ad)]/(c^2+d^2) //z=(a+bi)/(c+di)=[(ac+bd)+i(bc-ad)]/(c^2+d^2)
double den; float den;
for(int i=0;i<kf_data.rows;i++){ for(int i=0;i<kf_data.rows;i++){
for(int j=0;j<kf_data.cols;j++){ for(int j=0;j<kf_data.cols;j++){
den=1.0/(_alphaf_den.at<Vec2d>(i,j)[0]*_alphaf_den.at<Vec2d>(i,j)[0]+_alphaf_den.at<Vec2d>(i,j)[1]*_alphaf_den.at<Vec2d>(i,j)[1]); den=1.0f/(_alphaf_den.at<Vec2f>(i,j)[0]*_alphaf_den.at<Vec2f>(i,j)[0]+_alphaf_den.at<Vec2f>(i,j)[1]*_alphaf_den.at<Vec2f>(i,j)[1]);
spec2_data.at<Vec2d>(i,j)[0]= spec2_data.at<Vec2f>(i,j)[0]=
(spec_data.at<Vec2d>(i,j)[0]*_alphaf_den.at<Vec2d>(i,j)[0]+spec_data.at<Vec2d>(i,j)[1]*_alphaf_den.at<Vec2d>(i,j)[1])*den; (spec_data.at<Vec2f>(i,j)[0]*_alphaf_den.at<Vec2f>(i,j)[0]+spec_data.at<Vec2f>(i,j)[1]*_alphaf_den.at<Vec2f>(i,j)[1])*den;
spec2_data.at<Vec2d>(i,j)[1]= spec2_data.at<Vec2f>(i,j)[1]=
(spec_data.at<Vec2d>(i,j)[1]*_alphaf_den.at<Vec2d>(i,j)[0]-spec_data.at<Vec2d>(i,j)[0]*_alphaf_den.at<Vec2d>(i,j)[1])*den; (spec_data.at<Vec2f>(i,j)[1]*_alphaf_den.at<Vec2f>(i,j)[0]-spec_data.at<Vec2f>(i,j)[0]*_alphaf_den.at<Vec2f>(i,j)[1])*den;
} }
} }
...@@ -825,11 +896,11 @@ namespace cv{ ...@@ -825,11 +896,11 @@ namespace cv{
* Parameters * Parameters
*/ */
TrackerKCF::Params::Params(){ TrackerKCF::Params::Params(){
detect_thresh = 0.5; detect_thresh = 0.5f;
sigma=0.2; sigma=0.2f;
lambda=0.0001; lambda=0.0001f;
interp_factor=0.075; interp_factor=0.075f;
output_sigma_factor=1.0/16.0; output_sigma_factor=1.0f / 16.0f;
resize=true; resize=true;
max_patch_size=80*80; max_patch_size=80*80;
split_coeff=true; split_coeff=true;
...@@ -840,7 +911,7 @@ namespace cv{ ...@@ -840,7 +911,7 @@ namespace cv{
//feature compression //feature compression
compress_feature=true; compress_feature=true;
compressed_size=2; compressed_size=2;
pca_learning_rate=0.15; pca_learning_rate=0.15f;
} }
void TrackerKCF::Params::read( const cv::FileNode& fn ){ void TrackerKCF::Params::read( const cv::FileNode& fn ){
......
...@@ -63,10 +63,10 @@ TEST(KCF_Parameters, IO) ...@@ -63,10 +63,10 @@ TEST(KCF_Parameters, IO)
{ {
TrackerKCF::Params parameters; TrackerKCF::Params parameters;
parameters.sigma = 0.3; parameters.sigma = 0.3f;
parameters.lambda = 0.02; parameters.lambda = 0.02f;
parameters.interp_factor = 0.08; parameters.interp_factor = 0.08f;
parameters.output_sigma_factor = 1.0/ 32.0; parameters.output_sigma_factor = 1.0f/ 32.0f;
parameters.resize=false; parameters.resize=false;
parameters.max_patch_size=90*90; parameters.max_patch_size=90*90;
parameters.split_coeff=false; parameters.split_coeff=false;
...@@ -75,7 +75,7 @@ TEST(KCF_Parameters, IO) ...@@ -75,7 +75,7 @@ TEST(KCF_Parameters, IO)
parameters.desc_pca = TrackerKCF::GRAY; parameters.desc_pca = TrackerKCF::GRAY;
parameters.compress_feature=false; parameters.compress_feature=false;
parameters.compressed_size=3; parameters.compressed_size=3;
parameters.pca_learning_rate=0.2; parameters.pca_learning_rate=0.2f;
FileStorage fsWriter("parameters.xml", FileStorage::WRITE + FileStorage::MEMORY); FileStorage fsWriter("parameters.xml", FileStorage::WRITE + FileStorage::MEMORY);
parameters.write(fsWriter); parameters.write(fsWriter);
......
...@@ -605,7 +605,7 @@ public: ...@@ -605,7 +605,7 @@ public:
* @brief Weights (multiplicative constants) that linearly stretch individual axes of the feature space * @brief Weights (multiplicative constants) that linearly stretch individual axes of the feature space
* (x,y = position; L,a,b = color in CIE Lab space; c = contrast. e = entropy) * (x,y = position; L,a,b = color in CIE Lab space; c = contrast. e = entropy)
*/ */
CV_WRAP virtual float getWeightConstrast() const = 0; CV_WRAP virtual float getWeightContrast() const = 0;
/** /**
* @brief Weights (multiplicative constants) that linearly stretch individual axes of the feature space * @brief Weights (multiplicative constants) that linearly stretch individual axes of the feature space
* (x,y = position; L,a,b = color in CIE Lab space; c = contrast. e = entropy) * (x,y = position; L,a,b = color in CIE Lab space; c = contrast. e = entropy)
...@@ -925,6 +925,26 @@ public: ...@@ -925,6 +925,26 @@ public:
bool useProvidedKeypoints=false ) = 0; bool useProvidedKeypoints=false ) = 0;
}; };
/** @brief Estimates cornerness for prespecified KeyPoints using the FAST algorithm
@param image grayscale image where keypoints (corners) are detected.
@param keypoints keypoints which should be tested to fit the FAST criteria. Keypoints not beeing
detected as corners are removed.
@param threshold threshold on difference between intensity of the central pixel and pixels of a
circle around this pixel.
@param nonmaxSuppression if true, non-maximum suppression is applied to detected corners
(keypoints).
@param type one of the three neighborhoods as defined in the paper:
FastFeatureDetector::TYPE_9_16, FastFeatureDetector::TYPE_7_12,
FastFeatureDetector::TYPE_5_8
Detects corners using the FAST algorithm by @cite Rosten06 .
*/
CV_EXPORTS void FASTForPointSet( InputArray image, CV_IN_OUT std::vector<KeyPoint>& keypoints,
int threshold, bool nonmaxSuppression=true, int type=FastFeatureDetector::TYPE_9_16);
//! @} //! @}
} }
......
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.
#include <opencv2/xfeatures2d.hpp>
#ifndef VERIFY_CORNERS
#define VERIFY_CORNERS 0
#endif
namespace {
using namespace cv;
#if VERIFY_CORNERS
void testCorner(const uchar* ptr, const int pixel[], int K, int N, int threshold) {
// check that with the computed "threshold" the pixel is still a corner
// and that with the increased-by-1 "threshold" the pixel is not a corner anymore
for( int delta = 0; delta <= 1; delta++ )
{
int v0 = std::min(ptr[0] + threshold + delta, 255);
int v1 = std::max(ptr[0] - threshold - delta, 0);
int c0 = 0, c1 = 0;
for( int k = 0; k < N; k++ )
{
int x = ptr[pixel[k]];
if(x > v0)
{
if( ++c0 > K )
break;
c1 = 0;
}
else if( x < v1 )
{
if( ++c1 > K )
break;
c0 = 0;
}
else
{
c0 = c1 = 0;
}
}
CV_Assert( (delta == 0 && std::max(c0, c1) > K) ||
(delta == 1 && std::max(c0, c1) <= K) );
}
}
#endif
template<int patternSize>
int cornerScore(const uchar* ptr, const int pixel[], int threshold);
template<>
int cornerScore<16>(const uchar* ptr, const int pixel[], int threshold)
{
const int K = 8, N = K*3 + 1;
int k, v = ptr[0];
short d[N];
for( k = 0; k < N; k++ )
d[k] = (short)(v - ptr[pixel[k]]);
#if CV_SSE2
__m128i q0 = _mm_set1_epi16(-1000), q1 = _mm_set1_epi16(1000);
for( k = 0; k < 16; k += 8 )
{
__m128i v0 = _mm_loadu_si128((__m128i*)(d+k+1));
__m128i v1 = _mm_loadu_si128((__m128i*)(d+k+2));
__m128i a = _mm_min_epi16(v0, v1);
__m128i b = _mm_max_epi16(v0, v1);
v0 = _mm_loadu_si128((__m128i*)(d+k+3));
a = _mm_min_epi16(a, v0);
b = _mm_max_epi16(b, v0);
v0 = _mm_loadu_si128((__m128i*)(d+k+4));
a = _mm_min_epi16(a, v0);
b = _mm_max_epi16(b, v0);
v0 = _mm_loadu_si128((__m128i*)(d+k+5));
a = _mm_min_epi16(a, v0);
b = _mm_max_epi16(b, v0);
v0 = _mm_loadu_si128((__m128i*)(d+k+6));
a = _mm_min_epi16(a, v0);
b = _mm_max_epi16(b, v0);
v0 = _mm_loadu_si128((__m128i*)(d+k+7));
a = _mm_min_epi16(a, v0);
b = _mm_max_epi16(b, v0);
v0 = _mm_loadu_si128((__m128i*)(d+k+8));
a = _mm_min_epi16(a, v0);
b = _mm_max_epi16(b, v0);
v0 = _mm_loadu_si128((__m128i*)(d+k));
q0 = _mm_max_epi16(q0, _mm_min_epi16(a, v0));
q1 = _mm_min_epi16(q1, _mm_max_epi16(b, v0));
v0 = _mm_loadu_si128((__m128i*)(d+k+9));
q0 = _mm_max_epi16(q0, _mm_min_epi16(a, v0));
q1 = _mm_min_epi16(q1, _mm_max_epi16(b, v0));
}
q0 = _mm_max_epi16(q0, _mm_sub_epi16(_mm_setzero_si128(), q1));
q0 = _mm_max_epi16(q0, _mm_unpackhi_epi64(q0, q0));
q0 = _mm_max_epi16(q0, _mm_srli_si128(q0, 4));
q0 = _mm_max_epi16(q0, _mm_srli_si128(q0, 2));
threshold = (short)_mm_cvtsi128_si32(q0) - 1;
#else
int a0 = threshold;
for( k = 0; k < 16; k += 2 )
{
int a = std::min((int)d[k+1], (int)d[k+2]);
a = std::min(a, (int)d[k+3]);
if( a <= a0 )
continue;
a = std::min(a, (int)d[k+4]);
a = std::min(a, (int)d[k+5]);
a = std::min(a, (int)d[k+6]);
a = std::min(a, (int)d[k+7]);
a = std::min(a, (int)d[k+8]);
a0 = std::max(a0, std::min(a, (int)d[k]));
a0 = std::max(a0, std::min(a, (int)d[k+9]));
}
int b0 = -a0;
for( k = 0; k < 16; k += 2 )
{
int b = std::max((int)d[k+1], (int)d[k+2]);
b = std::max(b, (int)d[k+3]);
b = std::max(b, (int)d[k+4]);
b = std::max(b, (int)d[k+5]);
if( b >= b0 )
continue;
b = std::max(b, (int)d[k+6]);
b = std::max(b, (int)d[k+7]);
b = std::max(b, (int)d[k+8]);
b0 = std::min(b0, std::max(b, (int)d[k]));
b0 = std::min(b0, std::max(b, (int)d[k+9]));
}
threshold = -b0-1;
#endif
#if VERIFY_CORNERS
testCorner(ptr, pixel, K, N, threshold);
#endif
return threshold;
}
template<>
int cornerScore<12>(const uchar* ptr, const int pixel[], int threshold)
{
const int K = 6, N = K*3 + 1;
int k, v = ptr[0];
short d[N + 4];
for( k = 0; k < N; k++ )
d[k] = (short)(v - ptr[pixel[k]]);
#if CV_SSE2
for( k = 0; k < 4; k++ )
d[N+k] = d[k];
#endif
#if CV_SSE2
__m128i q0 = _mm_set1_epi16(-1000), q1 = _mm_set1_epi16(1000);
for( k = 0; k < 16; k += 8 )
{
__m128i v0 = _mm_loadu_si128((__m128i*)(d+k+1));
__m128i v1 = _mm_loadu_si128((__m128i*)(d+k+2));
__m128i a = _mm_min_epi16(v0, v1);
__m128i b = _mm_max_epi16(v0, v1);
v0 = _mm_loadu_si128((__m128i*)(d+k+3));
a = _mm_min_epi16(a, v0);
b = _mm_max_epi16(b, v0);
v0 = _mm_loadu_si128((__m128i*)(d+k+4));
a = _mm_min_epi16(a, v0);
b = _mm_max_epi16(b, v0);
v0 = _mm_loadu_si128((__m128i*)(d+k+5));
a = _mm_min_epi16(a, v0);
b = _mm_max_epi16(b, v0);
v0 = _mm_loadu_si128((__m128i*)(d+k+6));
a = _mm_min_epi16(a, v0);
b = _mm_max_epi16(b, v0);
v0 = _mm_loadu_si128((__m128i*)(d+k));
q0 = _mm_max_epi16(q0, _mm_min_epi16(a, v0));
q1 = _mm_min_epi16(q1, _mm_max_epi16(b, v0));
v0 = _mm_loadu_si128((__m128i*)(d+k+7));
q0 = _mm_max_epi16(q0, _mm_min_epi16(a, v0));
q1 = _mm_min_epi16(q1, _mm_max_epi16(b, v0));
}
q0 = _mm_max_epi16(q0, _mm_sub_epi16(_mm_setzero_si128(), q1));
q0 = _mm_max_epi16(q0, _mm_unpackhi_epi64(q0, q0));
q0 = _mm_max_epi16(q0, _mm_srli_si128(q0, 4));
q0 = _mm_max_epi16(q0, _mm_srli_si128(q0, 2));
threshold = (short)_mm_cvtsi128_si32(q0) - 1;
#else
int a0 = threshold;
for( k = 0; k < 12; k += 2 )
{
int a = std::min((int)d[k+1], (int)d[k+2]);
if( a <= a0 )
continue;
a = std::min(a, (int)d[k+3]);
a = std::min(a, (int)d[k+4]);
a = std::min(a, (int)d[k+5]);
a = std::min(a, (int)d[k+6]);
a0 = std::max(a0, std::min(a, (int)d[k]));
a0 = std::max(a0, std::min(a, (int)d[k+7]));
}
int b0 = -a0;
for( k = 0; k < 12; k += 2 )
{
int b = std::max((int)d[k+1], (int)d[k+2]);
b = std::max(b, (int)d[k+3]);
b = std::max(b, (int)d[k+4]);
if( b >= b0 )
continue;
b = std::max(b, (int)d[k+5]);
b = std::max(b, (int)d[k+6]);
b0 = std::min(b0, std::max(b, (int)d[k]));
b0 = std::min(b0, std::max(b, (int)d[k+7]));
}
threshold = -b0-1;
#endif
#if VERIFY_CORNERS
testCorner(ptr, pixel, K, N, threshold);
#endif
return threshold;
}
template<>
int cornerScore<8>(const uchar* ptr, const int pixel[], int threshold)
{
const int K = 4, N = K*3 + 1;
int k, v = ptr[0];
short d[N];
for( k = 0; k < N; k++ )
d[k] = (short)(v - ptr[pixel[k]]);
#if CV_SSE2
__m128i v0 = _mm_loadu_si128((__m128i*)(d+1));
__m128i v1 = _mm_loadu_si128((__m128i*)(d+2));
__m128i a = _mm_min_epi16(v0, v1);
__m128i b = _mm_max_epi16(v0, v1);
v0 = _mm_loadu_si128((__m128i*)(d+3));
a = _mm_min_epi16(a, v0);
b = _mm_max_epi16(b, v0);
v0 = _mm_loadu_si128((__m128i*)(d+4));
a = _mm_min_epi16(a, v0);
b = _mm_max_epi16(b, v0);
v0 = _mm_loadu_si128((__m128i*)(d));
__m128i q0 = _mm_min_epi16(a, v0);
__m128i q1 = _mm_max_epi16(b, v0);
v0 = _mm_loadu_si128((__m128i*)(d+5));
q0 = _mm_max_epi16(q0, _mm_min_epi16(a, v0));
q1 = _mm_min_epi16(q1, _mm_max_epi16(b, v0));
q0 = _mm_max_epi16(q0, _mm_sub_epi16(_mm_setzero_si128(), q1));
q0 = _mm_max_epi16(q0, _mm_unpackhi_epi64(q0, q0));
q0 = _mm_max_epi16(q0, _mm_srli_si128(q0, 4));
q0 = _mm_max_epi16(q0, _mm_srli_si128(q0, 2));
threshold = (short)_mm_cvtsi128_si32(q0) - 1;
#else
int a0 = threshold;
for( k = 0; k < 8; k += 2 )
{
int a = std::min((int)d[k+1], (int)d[k+2]);
if( a <= a0 )
continue;
a = std::min(a, (int)d[k+3]);
a = std::min(a, (int)d[k+4]);
a0 = std::max(a0, std::min(a, (int)d[k]));
a0 = std::max(a0, std::min(a, (int)d[k+5]));
}
int b0 = -a0;
for( k = 0; k < 8; k += 2 )
{
int b = std::max((int)d[k+1], (int)d[k+2]);
b = std::max(b, (int)d[k+3]);
if( b >= b0 )
continue;
b = std::max(b, (int)d[k+4]);
b0 = std::min(b0, std::max(b, (int)d[k]));
b0 = std::min(b0, std::max(b, (int)d[k+5]));
}
threshold = -b0-1;
#endif
#if VERIFY_CORNERS
testCorner(ptr, pixel, K, N, threshold);
#endif
return threshold;
}
void makeOffsets(int pixel[25], int rowStride, int patternSize)
{
static const int offsets16[][2] =
{
{0, 3}, { 1, 3}, { 2, 2}, { 3, 1}, { 3, 0}, { 3, -1}, { 2, -2}, { 1, -3},
{0, -3}, {-1, -3}, {-2, -2}, {-3, -1}, {-3, 0}, {-3, 1}, {-2, 2}, {-1, 3}
};
static const int offsets12[][2] =
{
{0, 2}, { 1, 2}, { 2, 1}, { 2, 0}, { 2, -1}, { 1, -2},
{0, -2}, {-1, -2}, {-2, -1}, {-2, 0}, {-2, 1}, {-1, 2}
};
static const int offsets8[][2] =
{
{0, 1}, { 1, 1}, { 1, 0}, { 1, -1},
{0, -1}, {-1, -1}, {-1, 0}, {-1, 1}
};
const int (*offsets)[2] = patternSize == 16 ? offsets16 :
patternSize == 12 ? offsets12 :
patternSize == 8 ? offsets8 : 0;
CV_Assert(pixel && offsets);
int k = 0;
for( ; k < patternSize; k++ )
pixel[k] = offsets[k][0] + offsets[k][1] * rowStride;
for( ; k < 25; k++ )
pixel[k] = pixel[k - patternSize];
}
template<int patternSize>
void FASTForPointSet_t( InputArray image, std::vector<KeyPoint>& keypoints, int threshold, bool nonmaxSuppression ) {
Mat img = image.getMat();
const int K = patternSize/2, N = patternSize + K + 1;
int i, k, pixel[25];
makeOffsets(pixel, (int)img.step, patternSize);
keypoints.clear();
threshold = std::min(std::max(threshold, 0), 255);
uchar threshold_tab[512];
for( i = -255; i <= 255; i++ )
threshold_tab[i+255] = (uchar)(i < -threshold ? 1 : i > threshold ? 2 : 0);
AutoBuffer<uchar> _buf((img.cols+16)*3*(sizeof(int) + sizeof(uchar)) + 128);
uchar* buf[3];
buf[0] = _buf; buf[1] = buf[0] + img.cols; buf[2] = buf[1] + img.cols;
int* cpbuf[3];
cpbuf[0] = (int*)alignPtr(buf[2] + img.cols, sizeof(int)) + 1;
cpbuf[1] = cpbuf[0] + img.cols + 1;
cpbuf[2] = cpbuf[1] + img.cols + 1;
memset(buf[0], 0, img.cols*3);
// Calculate threshold for the keypoints
for (size_t keyPointIdx=0; keyPointIdx < keypoints.size(); keyPointIdx++) {
// Set response to -1:
// All keypoints with response <= 0 will be removed afterwards
keypoints[keyPointIdx].response = -1;
// Poiter to keyPoint in image
Point keyPoint = keypoints[keyPointIdx].pt;
const uchar* ptr = img.ptr<uchar>(keyPoint.y, keyPoint.x);
// value of the pixel at certain position
int v = ptr[0];
// Initialize Lookup table
// If k=v --> tab[k] is at the center of the thrshold table
// The threshold table is made as follows:
// -255 -threshold 0 +threshold 255
// 111111111111111111|0000000000000|0000000000000|222222222222222
const uchar* tab = &threshold_tab[0] - v + 255;
// Calculate the fast value
int d = tab[ptr[pixel[0]]] | tab[ptr[pixel[8]]];
if( d == 0 )
continue;
d &= tab[ptr[pixel[2]]] | tab[ptr[pixel[10]]];
d &= tab[ptr[pixel[4]]] | tab[ptr[pixel[12]]];
d &= tab[ptr[pixel[6]]] | tab[ptr[pixel[14]]];
if( d == 0 )
continue;
d &= tab[ptr[pixel[1]]] | tab[ptr[pixel[9]]];
d &= tab[ptr[pixel[3]]] | tab[ptr[pixel[11]]];
d &= tab[ptr[pixel[5]]] | tab[ptr[pixel[13]]];
d &= tab[ptr[pixel[7]]] | tab[ptr[pixel[15]]];
// For at least half pixels darker than v count the number
if( d & 1 )
{
int vt = v - threshold, count = 0;
for(k = 0; k < N; k++ )
{
int x = ptr[pixel[k]];
if(x < vt)
{
if( ++count > K )
{
// Calculate score
keypoints[keyPointIdx].response = (uchar)cornerScore<patternSize>(ptr, pixel, threshold);
// Non Maxima Supression I
if (nonmaxSuppression && keyPointIdx>0 && keypoints[keyPointIdx-1].response < keypoints[keyPointIdx].response) {
keypoints[keyPointIdx-1].response = -1;
}
break;
}
}
else
count = 0;
}
}
// For at least half pixels brighter than v count the number
if(d & 2 )
{
int vt = v + threshold, count = 0;
for(k = 0; k < N; k++ )
{
int x = ptr[pixel[k]];
if(x > vt)
{
if( ++count > K )
{
// Calculate score
keypoints[keyPointIdx].response = (uchar)cornerScore<patternSize>(ptr, pixel, threshold);
// Non Maxima Suppression I
if (nonmaxSuppression && keyPointIdx>0 &&keypoints[keyPointIdx-1].response < keypoints[keyPointIdx].response) {
keypoints[keyPointIdx-1].response = -1;
}
break;
}
}
else
count = 0;
}
}
}
// Remove unused Keypoints
size_t maxKeypointSize = keypoints.size();
for (size_t keyPointIdx=maxKeypointSize; keyPointIdx > 0;) {
keyPointIdx--;
if (keypoints[keyPointIdx].response <= 0) {
keypoints.erase(keypoints.begin() + keyPointIdx);
} else if (nonmaxSuppression && keyPointIdx>0 && keypoints[keyPointIdx-1].response > keypoints[keyPointIdx].response) {
// Non Maxima Suppression II
keypoints.erase(keypoints.begin() + keyPointIdx);
}
}
}
}
namespace cv {
namespace xfeatures2d {
void FASTForPointSet(InputArray _img, std::vector<KeyPoint>& keypoints, int threshold, bool nonmax_suppression, int type)
{
if (keypoints.empty()) {
FAST(_img, keypoints, threshold, nonmax_suppression, type);
return;
}
switch(type) {
case FastFeatureDetector::TYPE_5_8:
FASTForPointSet_t<8>(_img, keypoints, threshold, nonmax_suppression);
break;
case FastFeatureDetector::TYPE_7_12:
FASTForPointSet_t<12>(_img, keypoints, threshold, nonmax_suppression);
break;
case FastFeatureDetector::TYPE_9_16:
FASTForPointSet_t<16>(_img, keypoints, threshold, nonmax_suppression);
break;
}
}
}
}
...@@ -137,7 +137,7 @@ namespace cv ...@@ -137,7 +137,7 @@ namespace cv
float getWeightL() const { return mSampler->getWeightL(); } float getWeightL() const { return mSampler->getWeightL(); }
float getWeightA() const { return mSampler->getWeightA(); } float getWeightA() const { return mSampler->getWeightA(); }
float getWeightB() const { return mSampler->getWeightB(); } float getWeightB() const { return mSampler->getWeightB(); }
float getWeightConstrast() const { return mSampler->getWeightConstrast(); } float getWeightContrast() const { return mSampler->getWeightContrast(); }
float getWeightEntropy() const { return mSampler->getWeightEntropy(); } float getWeightEntropy() const { return mSampler->getWeightEntropy(); }
std::vector<Point2f> getSamplingPoints() const { return mSampler->getSamplingPoints(); } std::vector<Point2f> getSamplingPoints() const { return mSampler->getSamplingPoints(); }
......
...@@ -128,7 +128,7 @@ namespace cv ...@@ -128,7 +128,7 @@ namespace cv
float getWeightL() const { return mWeights[L_IDX]; } float getWeightL() const { return mWeights[L_IDX]; }
float getWeightA() const { return mWeights[A_IDX]; } float getWeightA() const { return mWeights[A_IDX]; }
float getWeightB() const { return mWeights[B_IDX]; } float getWeightB() const { return mWeights[B_IDX]; }
float getWeightConstrast() const { return mWeights[CONTRAST_IDX]; } float getWeightContrast() const { return mWeights[CONTRAST_IDX]; }
float getWeightEntropy() const { return mWeights[ENTROPY_IDX]; } float getWeightEntropy() const { return mWeights[ENTROPY_IDX]; }
std::vector<Point2f> getSamplingPoints() const std::vector<Point2f> getSamplingPoints() const
......
...@@ -103,7 +103,7 @@ namespace cv ...@@ -103,7 +103,7 @@ namespace cv
virtual float getWeightL() const = 0; virtual float getWeightL() const = 0;
virtual float getWeightA() const = 0; virtual float getWeightA() const = 0;
virtual float getWeightB() const = 0; virtual float getWeightB() const = 0;
virtual float getWeightConstrast() const = 0; virtual float getWeightContrast() const = 0;
virtual float getWeightEntropy() const = 0; virtual float getWeightEntropy() const = 0;
virtual std::vector<Point2f> getSamplingPoints() const = 0; virtual std::vector<Point2f> getSamplingPoints() const = 0;
......
...@@ -89,7 +89,7 @@ namespace cv ...@@ -89,7 +89,7 @@ namespace cv
const Mat& points2, int idx2) const Mat& points2, int idx2)
{ {
float distance = computeDistance(distancefunction, points1, idx1, points2, idx2); float distance = computeDistance(distancefunction, points1, idx1, points2, idx2);
return exp(-alpha + distance * distance); return exp(-alpha * distance * distance);
} }
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment