diff --git a/modules/ccalib/CMakeLists.txt b/modules/ccalib/CMakeLists.txt new file mode 100644 index 000000000..add019d78 --- /dev/null +++ b/modules/ccalib/CMakeLists.txt @@ -0,0 +1,2 @@ +set(the_description "Custom Calibration Pattern") +ocv_define_module(ccalib opencv_core opencv_imgproc opencv_calib3d opencv_features2d) diff --git a/modules/ccalib/doc/customPattern.rst b/modules/ccalib/doc/customPattern.rst new file mode 100644 index 000000000..bf7c97750 --- /dev/null +++ b/modules/ccalib/doc/customPattern.rst @@ -0,0 +1,202 @@ +Custom Calibration Pattern +========================== + +.. highlight:: cpp + +CustomPattern +------------- +A custom pattern class that can be used to calibrate a camera and to further track the translation and rotation of the pattern. Defaultly it uses an ``ORB`` feature detector and a ``BruteForce-Hamming(2)`` descriptor matcher to find the location of the pattern feature points that will subsequently be used for calibration. + +.. ocv:class:: CustomPattern : public Algorithm + + +CustomPattern::CustomPattern +---------------------------- +CustomPattern constructor. + +.. ocv:function:: CustomPattern() + + +CustomPattern::create +--------------------- +A method that initializes the class and generates the necessary detectors, extractors and matchers. + +.. ocv:function:: bool create(InputArray pattern, const Size2f boardSize, OutputArray output = noArray()) + + :param pattern: The image, which will be used as a pattern. If the desired pattern is part of a bigger image, you can crop it out using image(roi). + + :param boardSize: The size of the pattern in physical dimensions. These will be used to scale the points when the calibration occurs. + + :param output: A matrix that is the same as the input pattern image, but has all the feature points drawn on it. + + :return Returns whether the initialization was successful or not. Possible reason for failure may be that no feature points were detected. + +.. seealso:: + + :ocv:func:`getFeatureDetector`, + :ocv:func:`getDescriptorExtractor`, + :ocv:func:`getDescriptorMatcher` + +.. note:: + + * Determine the number of detected feature points can be done through :ocv:func:`getPatternPoints` method. + + * The feature detector, extractor and matcher cannot be changed after initialization. + + + +CustomPattern::findPattern +-------------------------- +Finds the pattern in the input image + +.. ocv:function:: bool findPattern(InputArray image, OutputArray matched_features, OutputArray pattern_points, const double ratio = 0.7, const double proj_error = 8.0, const bool refine_position = false, OutputArray out = noArray(), OutputArray H = noArray(), OutputArray pattern_corners = noArray()); + + :param image: The input image where the pattern is searched for. + + :param matched_features: A ``vector`` of the projections of calibration pattern points, matched in the image. The points correspond to the ``pattern_points``.``matched_features`` and ``pattern_points`` have the same size. + + :param pattern_points: A ``vector`` of calibration pattern points in the calibration pattern coordinate space. + + :param ratio: A ratio used to threshold matches based on D. Lowe's point ratio test. + + :param proj_error: The maximum projection error that is allowed when the found points are back projected. A lower projection error will be beneficial for eliminating mismatches. Higher values are recommended when the camera lens has greater distortions. + + :param refine_position: Whether to refine the position of the feature points with :ocv:func:`cornerSubPix`. + + :param out: An image showing the matched feature points and a contour around the estimated pattern. + + :param H: The homography transformation matrix between the pattern and the current image. + + :param pattern_corners: A ``vector`` containing the 4 corners of the found pattern. + + :return The method return whether the pattern was found or not. + + +CustomPattern::isInitialized +---------------------------- + +.. ocv:function:: bool isInitialized() + + :return If the class is initialized or not. + + +CustomPattern::getPatternPoints +------------------------------- + +.. ocv:function:: void getPatternPoints(OutputArray original_points) + + :param original_points: Fills the vector with the points found in the pattern. + + +CustomPattern::getPixelSize +--------------------------- +.. ocv:function:: double getPixelSize() + + :return Get the physical pixel size as initialized by the pattern. + + +CustomPattern::setFeatureDetector +--------------------------------- + .. ocv:function:: bool setFeatureDetector(Ptr featureDetector) + + :param featureDetector: Set a new FeatureDetector. + + :return Is it successfully set? Will fail if the object is already initialized by :ocv:func:`create`. + +.. note:: + + * It is left to user discretion to select matching feature detector, extractor and matchers. Please consult the documentation for each to confirm coherence. + + +CustomPattern::setDescriptorExtractor +------------------------------------- +.. ocv:function:: bool setDescriptorExtractor(Ptr extractor) + + :param extractor: Set a new DescriptorExtractor. + + :return Is it successfully set? Will fail if the object is already initialized by :ocv:func:`create`. + + +CustomPattern::setDescriptorMatcher +----------------------------------- +.. ocv:function:: bool setDescriptorMatcher(Ptr matcher) + + :param matcher: Set a new DescriptorMatcher. + + :return Is it successfully set? Will fail if the object is already initialized by :ocv:func:`create`. + + +CustomPattern::getFeatureDetector +--------------------------------- +.. ocv:function:: Ptr getFeatureDetector() + + :return The used FeatureDetector. + + +CustomPattern::getDescriptorExtractor +------------------------------------- +.. ocv:function:: Ptr getDescriptorExtractor() + + :return The used DescriptorExtractor. + + +CustomPattern::getDescriptorMatcher +----------------------------------- +.. ocv:function:: Ptr getDescriptorMatcher() + + :return The used DescriptorMatcher. + + +CustomPattern::calibrate +------------------------ +Calibrates the camera. + +.. ocv:function:: double calibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, Size imageSize, InputOutputArray cameraMatrix, InputOutputArray distCoeffs, OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, int flags = 0, TermCriteria criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON)) + + See :ocv:func:`calibrateCamera` for parameter information. + + +CustomPattern::findRt +--------------------- +Finds the rotation and translation vectors of the pattern. + +.. ocv:function:: bool findRt(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess = false, int flags = ITERATIVE) +.. ocv:function:: bool findRt(InputArray image, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess = false, int flags = ITERATIVE) + + :param image: The image, in which the rotation and translation of the pattern will be found. + + See :ocv:func:`solvePnP` for parameter information. + + +CustomPattern::findRtRANSAC +--------------------------- +Finds the rotation and translation vectors of the pattern using RANSAC. + +.. ocv:function:: bool findRtRANSAC(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess = false, int iterationsCount = 100, float reprojectionError = 8.0, int minInliersCount = 100, OutputArray inliers = noArray(), int flags = ITERATIVE) +.. ocv:function:: bool findRtRANSAC(InputArray image, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess = false, int iterationsCount = 100, float reprojectionError = 8.0, int minInliersCount = 100, OutputArray inliers = noArray(), int flags = ITERATIVE) + + :param image: The image, in which the rotation and translation of the pattern will be found. + + See :ocv:func:`solvePnPRANSAC` for parameter information. + + +CustomPattern::drawOrientation +------------------------------ +Draws the ``(x,y,z)`` axis on the image, in the center of the pattern, showing the orientation of the pattern. + +.. ocv:function:: void drawOrientation(InputOutputArray image, InputArray tvec, InputArray rvec, InputArray cameraMatrix, InputArray distCoeffs, double axis_length = 3, int axis_width = 2) + + :param image: The image, based on which the rotation and translation was calculated. The axis will be drawn in color - ``x`` - in red, ``y`` - in green, ``z`` - in blue. + + :param tvec: Translation vector. + + :param rvec: Rotation vector. + + :param cameraMatrix: The camera matrix. + + :param distCoeffs: The distortion coefficients. + + :param axis_length: The length of the axis symbol. + + :param axis_width: The width of the axis symbol. + diff --git a/modules/ccalib/include/opencv2/ccalib.hpp b/modules/ccalib/include/opencv2/ccalib.hpp new file mode 100644 index 000000000..53285e2be --- /dev/null +++ b/modules/ccalib/include/opencv2/ccalib.hpp @@ -0,0 +1,149 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// + // + // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. + // + // By downloading, copying, installing or using the software you agree to this license. + // If you do not agree to this license, do not download, install, + // copy or use the software. + // + // + // License Agreement + // For Open Source Computer Vision Library + // + // Copyright (C) 2014, OpenCV Foundation, all rights reserved. + // Third party copyrights are property of their respective owners. + // + // Redistribution and use in source and binary forms, with or without modification, + // are permitted provided that the following conditions are met: + // + // * Redistribution's of source code must retain the above copyright notice, + // this list of conditions and the following disclaimer. + // + // * Redistribution's in binary form must reproduce the above copyright notice, + // this list of conditions and the following disclaimer in the documentation + // and/or other materials provided with the distribution. + // + // * The name of the copyright holders may not be used to endorse or promote products + // derived from this software without specific prior written permission. + // + // This software is provided by the copyright holders and contributors "as is" and + // any express or implied warranties, including, but not limited to, the implied + // warranties of merchantability and fitness for a particular purpose are disclaimed. + // In no event shall the Intel Corporation or contributors be liable for any direct, + // indirect, incidental, special, exemplary, or consequential damages + // (including, but not limited to, procurement of substitute goods or services; + // loss of use, data, or profits; or business interruption) however caused + // and on any theory of liability, whether in contract, strict liability, + // or tort (including negligence or otherwise) arising in any way out of + // the use of this software, even if advised of the possibility of such damage. + // + //M*/ + +#ifndef __OPENCV_CCALIB_HPP__ +#define __OPENCV_CCALIB_HPP__ + +#include +#include +#include +#include + +#include + +namespace cv{ namespace ccalib{ + +class CV_EXPORTS CustomPattern : public Algorithm +{ +public: + CustomPattern(); + virtual ~CustomPattern(); + + bool create(InputArray pattern, const Size2f boardSize, OutputArray output = noArray()); + + bool findPattern(InputArray image, OutputArray matched_features, OutputArray pattern_points, const double ratio = 0.7, + const double proj_error = 8.0, const bool refine_position = false, OutputArray out = noArray(), + OutputArray H = noArray(), OutputArray pattern_corners = noArray()); + + bool isInitialized(); + + void getPatternPoints(OutputArray original_points); + /* + Returns a vector of the original points. + */ + double getPixelSize(); + /* + Get the pixel size of the pattern + */ + + bool setFeatureDetector(Ptr featureDetector); + bool setDescriptorExtractor(Ptr extractor); + bool setDescriptorMatcher(Ptr matcher); + + Ptr getFeatureDetector(); + Ptr getDescriptorExtractor(); + Ptr getDescriptorMatcher(); + + double calibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, + Size imageSize, InputOutputArray cameraMatrix, InputOutputArray distCoeffs, + OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, int flags = 0, + TermCriteria criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON)); + /* + Calls the calirateCamera function with the same inputs. + */ + + bool findRt(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, + OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess = false, int flags = SOLVEPNP_ITERATIVE); + bool findRt(InputArray image, InputArray cameraMatrix, InputArray distCoeffs, + OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess = false, int flags = SOLVEPNP_ITERATIVE); + /* + Uses solvePnP to find the rotation and translation of the pattern + with respect to the camera frame. + */ + + bool findRtRANSAC(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, + OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess = false, int iterationsCount = 100, + float reprojectionError = 8.0, int minInliersCount = 100, OutputArray inliers = noArray(), int flags = SOLVEPNP_ITERATIVE); + bool findRtRANSAC(InputArray image, InputArray cameraMatrix, InputArray distCoeffs, + OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess = false, int iterationsCount = 100, + float reprojectionError = 8.0, int minInliersCount = 100, OutputArray inliers = noArray(), int flags = SOLVEPNP_ITERATIVE); + /* + Uses solvePnPRansac() + */ + + void drawOrientation(InputOutputArray image, InputArray tvec, InputArray rvec, InputArray cameraMatrix, + InputArray distCoeffs, double axis_length = 3, int axis_width = 2); + /* + pattern_corners -> projected over the image position of the edges of the pattern. + */ + +private: + + Mat img_roi; + std::vector obj_corners; + double pxSize; + + bool initialized; + + Ptr detector; + Ptr descriptorExtractor; + Ptr descriptorMatcher; + + std::vector keypoints; + std::vector points3d; + Mat descriptor; + + bool init(Mat& image, const float pixel_size, OutputArray output = noArray()); + bool findPatternPass(const Mat& image, std::vector& matched_features, std::vector& pattern_points, + Mat& H, std::vector& scene_corners, const double pratio, const double proj_error, + const bool refine_position = false, const Mat& mask = Mat(), OutputArray output = noArray()); + void scaleFoundPoints(const double squareSize, const std::vector& corners, std::vector& pts3d); + void check_matches(std::vector& matched, const std::vector& pattern, std::vector& good, std::vector& pattern_3d, const Mat& H); + + void keypoints2points(const std::vector& in, std::vector& out); + void updateKeypointsPos(std::vector& in, const std::vector& new_pos); + void refinePointsPos(const Mat& img, std::vector& p); + void refineKeypointsPos(const Mat& img, std::vector& kp); +}; + +}} // namespace ccalib, cv + +#endif diff --git a/modules/ccalib/src/ccalib.cpp b/modules/ccalib/src/ccalib.cpp new file mode 100644 index 000000000..bd2e11cab --- /dev/null +++ b/modules/ccalib/src/ccalib.cpp @@ -0,0 +1,494 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// + // + // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. + // + // By downloading, copying, installing or using the software you agree to this license. + // If you do not agree to this license, do not download, install, + // copy or use the software. + // + // + // License Agreement + // For Open Source Computer Vision Library + // + // Copyright (C) 2014, OpenCV Foundation, all rights reserved. + // Third party copyrights are property of their respective owners. + // + // Redistribution and use in source and binary forms, with or without modification, + // are permitted provided that the following conditions are met: + // + // * Redistribution's of source code must retain the above copyright notice, + // this list of conditions and the following disclaimer. + // + // * Redistribution's in binary form must reproduce the above copyright notice, + // this list of conditions and the following disclaimer in the documentation + // and/or other materials provided with the distribution. + // + // * The name of the copyright holders may not be used to endorse or promote products + // derived from this software without specific prior written permission. + // + // This software is provided by the copyright holders and contributors "as is" and + // any express or implied warranties, including, but not limited to, the implied + // warranties of merchantability and fitness for a particular purpose are disclaimed. + // In no event shall the Intel Corporation or contributors be liable for any direct, + // indirect, incidental, special, exemplary, or consequential damages + // (including, but not limited to, procurement of substitute goods or services; + // loss of use, data, or profits; or business interruption) however caused + // and on any theory of liability, whether in contract, strict liability, + // or tort (including negligence or otherwise) arising in any way out of + // the use of this software, even if advised of the possibility of such damage. + // + //M*/ + +#ifndef __OPENCV_CCALIB_CPP__ +#define __OPENCV_CCALIB_CPP__ +#ifdef __cplusplus + +#include "precomp.hpp" +#include "opencv2/ccalib.hpp" + +#include +#include // CV_TERM +#include +#include +#include + +#include +#include + +namespace cv{ namespace ccalib{ + +using namespace std; + +const int MIN_CONTOUR_AREA_PX = 100; +const float MIN_CONTOUR_AREA_RATIO = 0.2f; +const float MAX_CONTOUR_AREA_RATIO = 5.0f; + +const int MIN_POINTS_FOR_H = 10; + +const float MAX_PROJ_ERROR_PX = 5.0f; + +CustomPattern::CustomPattern() +{ + initialized = false; +} + +bool CustomPattern::create(InputArray pattern, const Size2f boardSize, OutputArray output) +{ + CV_Assert(!pattern.empty() && (boardSize.area() > 0)); + + Mat img = pattern.getMat(); + float pixel_size = (boardSize.width > boardSize.height)? // Choose the longer side for more accurate calculation + float(img.cols) / boardSize.width: // width is longer + float(img.rows) / boardSize.height; // height is longer + return init(img, pixel_size, output); +} + +bool CustomPattern::init(Mat& image, const float pixel_size, OutputArray output) +{ + image.copyTo(img_roi); + //Setup object corners + obj_corners = std::vector(4); + obj_corners[0] = Point2f(0, 0); obj_corners[1] = Point2f(float(img_roi.cols), 0); + obj_corners[2] = Point2f(float(img_roi.cols), float(img_roi.rows)); obj_corners[3] = Point2f(0, float(img_roi.rows)); + + if (!detector) // if no detector chosen, use default + { + detector = FeatureDetector::create("ORB"); + detector->set("nFeatures", 2000); + detector->set("scaleFactor", 1.15); + detector->set("nLevels", 30); + } + + detector->detect(img_roi, keypoints); + if (keypoints.empty()) + { + initialized = false; + return initialized; + } + refineKeypointsPos(img_roi, keypoints); + + if (!descriptorExtractor) // if no extractor chosen, use default + descriptorExtractor = DescriptorExtractor::create("ORB"); + descriptorExtractor->compute(img_roi, keypoints, descriptor); + + if (!descriptorMatcher) + descriptorMatcher = DescriptorMatcher::create("BruteForce-Hamming(2)"); + + // Scale found points by pixelSize + pxSize = pixel_size; + scaleFoundPoints(pxSize, keypoints, points3d); + + if (output.needed()) + { + Mat out; + drawKeypoints(img_roi, keypoints, out, Scalar(0, 0, 255)); + out.copyTo(output); + } + + initialized = !keypoints.empty(); + return initialized; // initialized if any keypoints are found +} + +CustomPattern::~CustomPattern() {} + +bool CustomPattern::isInitialized() +{ + return initialized; +} + +bool CustomPattern::setFeatureDetector(Ptr featureDetector) +{ + if (!initialized) + { + this->detector = featureDetector; + return true; + } + else + return false; +} + +bool CustomPattern::setDescriptorExtractor(Ptr extractor) +{ + if (!initialized) + { + this->descriptorExtractor = extractor; + return true; + } + else + return false; +} + +bool CustomPattern::setDescriptorMatcher(Ptr matcher) +{ + if (!initialized) + { + this->descriptorMatcher = matcher; + return true; + } + else + return false; +} + +Ptr CustomPattern::getFeatureDetector() +{ + return detector; +} + +Ptr CustomPattern::getDescriptorExtractor() +{ + return descriptorExtractor; +} + +Ptr CustomPattern::getDescriptorMatcher() +{ + return descriptorMatcher; +} + +void CustomPattern::scaleFoundPoints(const double pixelSize, + const vector& corners, vector& pts3d) +{ + for (unsigned int i = 0; i < corners.size(); ++i) + { + pts3d.push_back(Point3f( + float(corners[i].pt.x * pixelSize), + float(corners[i].pt.y * pixelSize), + 0)); + } +} + +//Takes a descriptor and turns it into an (x,y) point +void CustomPattern::keypoints2points(const vector& in, vector& out) +{ + out.clear(); + out.reserve(in.size()); + for (size_t i = 0; i < in.size(); ++i) + { + out.push_back(in[i].pt); + } +} + +void CustomPattern::updateKeypointsPos(vector& in, const vector& new_pos) +{ + for (size_t i = 0; i < in.size(); ++i) + { + in[i].pt= new_pos[i]; + } +} + +void CustomPattern::refinePointsPos(const Mat& img, vector& p) +{ + Mat gray; + cvtColor(img, gray, COLOR_RGB2GRAY); + cornerSubPix(gray, p, Size(10, 10), Size(-1, -1), + TermCriteria(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS, 30, 0.1)); + +} + +void CustomPattern::refineKeypointsPos(const Mat& img, vector& kp) +{ + vector points; + keypoints2points(kp, points); + refinePointsPos(img, points); + updateKeypointsPos(kp, points); +} + +template +void deleteStdVecElem(vector& v, int idx) +{ + v[idx] = v.back(); + v.pop_back(); +} + +void CustomPattern::check_matches(vector& matched, const vector& pattern, vector& good, + vector& pattern_3d, const Mat& H) +{ + vector proj; + perspectiveTransform(pattern, proj, H); + + int deleted = 0; + double error_sum = 0; + double error_sum_filtered = 0; + for (uint i = 0; i < proj.size(); ++i) + { + double error = norm(matched[i] - proj[i]); + error_sum += error; + if (error >= MAX_PROJ_ERROR_PX) + { + deleteStdVecElem(good, i); + deleteStdVecElem(matched, i); + deleteStdVecElem(pattern_3d, i); + ++deleted; + } + else + { + error_sum_filtered += error; + } + } +} + +bool CustomPattern::findPatternPass(const Mat& image, vector& matched_features, vector& pattern_points, + Mat& H, vector& scene_corners, const double pratio, const double proj_error, + const bool refine_position, const Mat& mask, OutputArray output) +{ + if (!initialized) {return false; } + matched_features.clear(); + pattern_points.clear(); + + vector > matches; + vector f_keypoints; + Mat f_descriptor; + + detector->detect(image, f_keypoints, mask); + if (refine_position) refineKeypointsPos(image, f_keypoints); + + descriptorExtractor->compute(image, f_keypoints, f_descriptor); + descriptorMatcher->knnMatch(f_descriptor, descriptor, matches, 2); // k = 2; + vector good_matches; + vector obj_points; + + for(int i = 0; i < f_descriptor.rows; ++i) + { + if(matches[i][0].distance < pratio * matches[i][1].distance) + { + const DMatch& dm = matches[i][0]; + good_matches.push_back(dm); + // "keypoints1[matches[i].queryIdx] has a corresponding point in keypoints2[matches[i].trainIdx]" + matched_features.push_back(f_keypoints[dm.queryIdx].pt); + pattern_points.push_back(points3d[dm.trainIdx]); + obj_points.push_back(keypoints[dm.trainIdx].pt); + } + } + + if (good_matches.size() < MIN_POINTS_FOR_H) return false; + + Mat h_mask; + H = findHomography(obj_points, matched_features, RANSAC, proj_error, h_mask); + if (H.empty()) + { + // cout << "findHomography() returned empty Mat." << endl; + return false; + } + + for(unsigned int i = 0; i < good_matches.size(); ++i) + { + if(!h_mask.data[i]) + { + deleteStdVecElem(good_matches, i); + deleteStdVecElem(matched_features, i); + deleteStdVecElem(pattern_points, i); + } + } + + if (good_matches.empty()) return false; + + size_t numb_elem = good_matches.size(); + check_matches(matched_features, obj_points, good_matches, pattern_points, H); + if (good_matches.empty() || numb_elem < good_matches.size()) return false; + + // Get the corners from the image + scene_corners = vector(4); + perspectiveTransform(obj_corners, scene_corners, H); + + // Check correctnes of H + // Is it a convex hull? + bool cConvex = isContourConvex(scene_corners); + if (!cConvex) return false; + + // Is the hull too large or small? + double scene_area = contourArea(scene_corners); + if (scene_area < MIN_CONTOUR_AREA_PX) return false; + double ratio = scene_area/img_roi.size().area(); + if ((ratio < MIN_CONTOUR_AREA_RATIO) || + (ratio > MAX_CONTOUR_AREA_RATIO)) return false; + + // Is any of the projected points outside the hull? + for(unsigned int i = 0; i < good_matches.size(); ++i) + { + if(pointPolygonTest(scene_corners, f_keypoints[good_matches[i].queryIdx].pt, false) < 0) + { + deleteStdVecElem(good_matches, i); + deleteStdVecElem(matched_features, i); + deleteStdVecElem(pattern_points, i); + } + } + + if (output.needed()) + { + Mat out; + drawMatches(image, f_keypoints, img_roi, keypoints, good_matches, out); + // Draw lines between the corners (the mapped object in the scene - image_2 ) + line(out, scene_corners[0], scene_corners[1], Scalar(0, 255, 0), 2); + line(out, scene_corners[1], scene_corners[2], Scalar(0, 255, 0), 2); + line(out, scene_corners[2], scene_corners[3], Scalar(0, 255, 0), 2); + line(out, scene_corners[3], scene_corners[0], Scalar(0, 255, 0), 2); + out.copyTo(output); + } + + return (!good_matches.empty()); // return true if there are enough good matches +} + +bool CustomPattern::findPattern(InputArray image, OutputArray matched_features, OutputArray pattern_points, + const double ratio, const double proj_error, const bool refine_position, OutputArray out, + OutputArray H, OutputArray pattern_corners) +{ + CV_Assert(!image.empty() && proj_error > 0); + + Mat img = image.getMat(); + vector m_ftrs; + vector pattern_pts; + Mat _H; + vector scene_corners; + if (!findPatternPass(img, m_ftrs, pattern_pts, _H, scene_corners, 0.6, proj_error, refine_position)) + return false; // pattern not found + + Mat mask = Mat::zeros(img.size(), CV_8UC1); + vector > obj(1); + vector scorners_int(scene_corners.size()); + for (uint i = 0; i < scene_corners.size(); ++i) + scorners_int[i] = (Point)scene_corners[i]; // for drawContours + obj[0] = scorners_int; + drawContours(mask, obj, 0, Scalar(255), FILLED); + + // Second pass + Mat output; + if (!findPatternPass(img, m_ftrs, pattern_pts, _H, scene_corners, + ratio, proj_error, refine_position, mask, output)) + return false; // pattern not found + + Mat(m_ftrs).copyTo(matched_features); + Mat(pattern_pts).copyTo(pattern_points); + if (out.needed()) output.copyTo(out); + if (H.needed()) _H.copyTo(H); + if (pattern_corners.needed()) Mat(scene_corners).copyTo(pattern_corners); + + return (!m_ftrs.empty()); +} + +void CustomPattern::getPatternPoints(OutputArray original_points) +{ + return Mat(keypoints).copyTo(original_points); +} + +double CustomPattern::getPixelSize() +{ + return pxSize; +} + +double CustomPattern::calibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, + Size imageSize, InputOutputArray cameraMatrix, InputOutputArray distCoeffs, + OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, int flags, + TermCriteria criteria) +{ + return calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, + rvecs, tvecs, flags, criteria); +} + +bool CustomPattern::findRt(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, + InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess, int flags) +{ + return solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec, useExtrinsicGuess, flags); +} + +bool CustomPattern::findRt(InputArray image, InputArray cameraMatrix, InputArray distCoeffs, + OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess, int flags) +{ + vector imagePoints; + vector objectPoints; + + if (!findPattern(image, imagePoints, objectPoints)) + return false; + return solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec, useExtrinsicGuess, flags); +} + +bool CustomPattern::findRtRANSAC(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, + OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess, int iterationsCount, + float reprojectionError, int minInliersCount, OutputArray inliers, int flags) +{ + solvePnPRansac(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec, useExtrinsicGuess, + iterationsCount, reprojectionError, minInliersCount, inliers, flags); + return true; // for consistency with the other methods +} + +bool CustomPattern::findRtRANSAC(InputArray image, InputArray cameraMatrix, InputArray distCoeffs, + OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess, int iterationsCount, + float reprojectionError, int minInliersCount, OutputArray inliers, int flags) +{ + vector imagePoints; + vector objectPoints; + + if (!findPattern(image, imagePoints, objectPoints)) + return false; + solvePnPRansac(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec, useExtrinsicGuess, + iterationsCount, reprojectionError, minInliersCount, inliers, flags); + return true; +} + +void CustomPattern::drawOrientation(InputOutputArray image, InputArray tvec, InputArray rvec, + InputArray cameraMatrix, InputArray distCoeffs, + double axis_length, int axis_width) +{ + Point3f ptrCtr3d = Point3f(float((img_roi.cols * pxSize)/2.0), float((img_roi.rows * pxSize)/2.0), 0); + + vector axis(4); + float alen = float(axis_length * pxSize); + axis[0] = ptrCtr3d; + axis[1] = Point3f(alen, 0, 0) + ptrCtr3d; + axis[2] = Point3f(0, alen, 0) + ptrCtr3d; + axis[3] = Point3f(0, 0, -alen) + ptrCtr3d; + + vector proj_axis; + projectPoints(axis, rvec, tvec, cameraMatrix, distCoeffs, proj_axis); + + Mat img = image.getMat(); + line(img, proj_axis[0], proj_axis[1], Scalar(0, 0, 255), axis_width); // red + line(img, proj_axis[0], proj_axis[2], Scalar(0, 255, 0), axis_width); // green + line(img, proj_axis[0], proj_axis[3], Scalar(255, 0, 0), axis_width); // blue + + img.copyTo(image); +} + +}} // namespace ccalib, cv + +#endif // __OPENCV_CCALIB_CPP__ +#endif // cplusplus + diff --git a/modules/ccalib/src/precomp.hpp b/modules/ccalib/src/precomp.hpp new file mode 100644 index 000000000..f52c72719 --- /dev/null +++ b/modules/ccalib/src/precomp.hpp @@ -0,0 +1,51 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// + // + // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. + // + // By downloading, copying, installing or using the software you agree to this license. + // If you do not agree to this license, do not download, install, + // copy or use the software. + // + // + // License Agreement + // For Open Source Computer Vision Library + // + // Copyright (C) 2014, OpenCV Foundation, all rights reserved. + // Third party copyrights are property of their respective owners. + // + // Redistribution and use in source and binary forms, with or without modification, + // are permitted provided that the following conditions are met: + // + // * Redistribution's of source code must retain the above copyright notice, + // this list of conditions and the following disclaimer. + // + // * Redistribution's in binary form must reproduce the above copyright notice, + // this list of conditions and the following disclaimer in the documentation + // and/or other materials provided with the distribution. + // + // * The name of the copyright holders may not be used to endorse or promote products + // derived from this software without specific prior written permission. + // + // This software is provided by the copyright holders and contributors "as is" and + // any express or implied warranties, including, but not limited to, the implied + // warranties of merchantability and fitness for a particular purpose are disclaimed. + // In no event shall the Intel Corporation or contributors be liable for any direct, + // indirect, incidental, special, exemplary, or consequential damages + // (including, but not limited to, procurement of substitute goods or services; + // loss of use, data, or profits; or business interruption) however caused + // and on any theory of liability, whether in contract, strict liability, + // or tort (including negligence or otherwise) arising in any way out of + // the use of this software, even if advised of the possibility of such damage. + // + //M*/ + +#ifndef __OPENCV_CCALIB_PRECOMP__ +#define __OPENCV_CCALIB_PRECOMP__ + +#include +#include +#include + +#include + +#endif