diff --git a/modules/aruco/include/opencv2/aruco.hpp b/modules/aruco/include/opencv2/aruco.hpp index 98a411c5704..4a9675fa4e6 100644 --- a/modules/aruco/include/opencv2/aruco.hpp +++ b/modules/aruco/include/opencv2/aruco.hpp @@ -230,21 +230,19 @@ struct CV_EXPORTS_W DetectorParameters { * @param parameters marker detection parameters * @param rejectedImgPoints contains the imgPoints of those squares whose inner code has not a * correct codification. Useful for debugging purposes. - * @param cameraMatrix optional input 3x3 floating-point camera matrix - * \f$A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ - * @param distCoeff optional vector of distortion coefficients - * \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6],[s_1, s_2, s_3, s_4]])\f$ of 4, 5, 8 or 12 elements * * Performs marker detection in the input image. Only markers included in the specific dictionary * are searched. For each detected marker, it returns the 2D position of its corner in the image * and its corresponding identifier. * Note that this function does not perform pose estimation. - * @sa estimatePoseSingleMarkers, estimatePoseBoard + * @note The function does not correct lens distortion or takes it into account. It's recommended to undistort + * input image with corresponging camera model, if camera parameters are known + * @sa undistort, estimatePoseSingleMarkers, estimatePoseBoard * */ CV_EXPORTS_W void detectMarkers(InputArray image, const Ptr &dictionary, OutputArrayOfArrays corners, OutputArray ids, const Ptr ¶meters = DetectorParameters::create(), - OutputArrayOfArrays rejectedImgPoints = noArray(), InputArray cameraMatrix= noArray(), InputArray distCoeff= noArray()); + OutputArrayOfArrays rejectedImgPoints = noArray()); @@ -274,8 +272,9 @@ CV_EXPORTS_W void detectMarkers(InputArray image, const Ptr &diction * The marker corrdinate system is centered on the middle of the marker, with the Z axis * perpendicular to the marker plane. * The coordinates of the four corners of the marker in its own coordinate system are: - * (-markerLength/2, markerLength/2, 0), (markerLength/2, markerLength/2, 0), - * (markerLength/2, -markerLength/2, 0), (-markerLength/2, -markerLength/2, 0) + * (0, 0, 0), (markerLength, 0, 0), + * (markerLength, markerLength, 0), (0, markerLength, 0) + * @sa use cv::drawFrameAxes to get world coordinate system axis for object points */ CV_EXPORTS_W void estimatePoseSingleMarkers(InputArrayOfArrays corners, float markerLength, InputArray cameraMatrix, InputArray distCoeffs, @@ -318,7 +317,14 @@ class CV_EXPORTS_W Board { CV_WRAP void setIds(InputArray ids); /// array of object points of all the marker corners in the board - /// each marker include its 4 corners in CCW order. For M markers, the size is Mx4. + /// each marker include its 4 corners in this order: + ///- objPoints[i][0] - left-top point of i-th marker + ///- objPoints[i][1] - right-top point of i-th marker + ///- objPoints[i][2] - right-bottom point of i-th marker + ///- objPoints[i][3] - left-bottom point of i-th marker + /// + /// Markers are placed in a certain order - row by row, left to right in every row. + /// For M markers, the size is Mx4. CV_PROP std::vector< std::vector< Point3f > > objPoints; /// the dictionary of markers employed for this board @@ -327,6 +333,9 @@ class CV_EXPORTS_W Board { /// vector of the identifiers of the markers in the board (same size than objPoints) /// The identifiers refers to the board dictionary CV_PROP_RW std::vector< int > ids; + + /// coordinate of the bottom right corner of the board, is set when calling the function create() + CV_PROP Point3f rightBottomBorder; }; @@ -426,6 +435,7 @@ class CV_EXPORTS_W GridBoard : public Board { * Input markers that are not included in the board layout are ignored. * The function returns the number of markers from the input employed for the board pose estimation. * Note that returning a 0 means the pose has not been estimated. + * @sa use cv::drawFrameAxes to get world coordinate system axis for object points */ CV_EXPORTS_W int estimatePoseBoard(InputArrayOfArrays corners, InputArray ids, const Ptr &board, InputArray cameraMatrix, InputArray distCoeffs, InputOutputArray rvec, @@ -490,6 +500,7 @@ CV_EXPORTS_W void refineDetectedMarkers( * Given an array of detected marker corners and its corresponding ids, this functions draws * the markers in the image. The marker borders are painted and the markers identifiers if provided. * Useful for debugging purposes. + * */ CV_EXPORTS_W void drawDetectedMarkers(InputOutputArray image, InputArrayOfArrays corners, InputArray ids = noArray(), @@ -497,29 +508,6 @@ CV_EXPORTS_W void drawDetectedMarkers(InputOutputArray image, InputArrayOfArrays -/** - * @brief Draw coordinate system axis from pose estimation - * - * @param image input/output image. It must have 1 or 3 channels. The number of channels is not - * altered. - * @param cameraMatrix input 3x3 floating-point camera matrix - * \f$A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ - * @param distCoeffs vector of distortion coefficients - * \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6],[s_1, s_2, s_3, s_4]])\f$ of 4, 5, 8 or 12 elements - * @param rvec rotation vector of the coordinate system that will be drawn. (@sa Rodrigues). - * @param tvec translation vector of the coordinate system that will be drawn. - * @param length length of the painted axis in the same unit than tvec (usually in meters) - * - * Given the pose estimation of a marker or board, this function draws the axis of the world - * coordinate system, i.e. the system centered on the marker/board. Useful for debugging purposes. - * - * @deprecated use cv::drawFrameAxes - */ -CV_EXPORTS_W void drawAxis(InputOutputArray image, InputArray cameraMatrix, InputArray distCoeffs, - InputArray rvec, InputArray tvec, float length); - - - /** * @brief Draw a canonical marker image * diff --git a/modules/aruco/include/opencv2/aruco/charuco.hpp b/modules/aruco/include/opencv2/aruco/charuco.hpp index aa7a7c75e89..2417b2fd895 100644 --- a/modules/aruco/include/opencv2/aruco/charuco.hpp +++ b/modules/aruco/include/opencv2/aruco/charuco.hpp @@ -181,6 +181,7 @@ CV_EXPORTS_W int interpolateCornersCharuco(InputArrayOfArrays markerCorners, Inp * This function estimates a Charuco board pose from some detected corners. * The function checks if the input corners are enough and valid to perform pose estimation. * If pose estimation is valid, returns true, else returns false. + * @sa use cv::drawFrameAxes to get world coordinate system axis for object points */ CV_EXPORTS_W bool estimatePoseCharucoBoard(InputArray charucoCorners, InputArray charucoIds, const Ptr &board, InputArray cameraMatrix, @@ -275,6 +276,7 @@ CV_EXPORTS_W double calibrateCameraCharuco( * diamond. * @param cameraMatrix Optional camera calibration matrix. * @param distCoeffs Optional camera distortion coefficients. + * @param dictionary dictionary of markers indicating the type of markers. * * This function detects Diamond markers from the previous detected ArUco markers. The diamonds * are returned in the diamondCorners and diamondIds parameters. If camera calibration parameters @@ -285,7 +287,8 @@ CV_EXPORTS_W void detectCharucoDiamond(InputArray image, InputArrayOfArrays mark InputArray markerIds, float squareMarkerLengthRate, OutputArrayOfArrays diamondCorners, OutputArray diamondIds, InputArray cameraMatrix = noArray(), - InputArray distCoeffs = noArray()); + InputArray distCoeffs = noArray(), + Ptr dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::PREDEFINED_DICTIONARY_NAME::DICT_4X4_50)); diff --git a/modules/aruco/samples/calibrate_camera.cpp b/modules/aruco/samples/calibrate_camera.cpp index bf61e864167..3162ae7ca29 100644 --- a/modules/aruco/samples/calibrate_camera.cpp +++ b/modules/aruco/samples/calibrate_camera.cpp @@ -101,7 +101,7 @@ int main(int argc, char *argv[]) { if(parser.get("zt")) calibrationFlags |= CALIB_ZERO_TANGENT_DIST; if(parser.get("pc")) calibrationFlags |= CALIB_FIX_PRINCIPAL_POINT; - Ptr detectorParams; + Ptr detectorParams = aruco::DetectorParameters::create(); if(parser.has("dp")) { FileStorage fs(parser.get("dp"), FileStorage::READ); bool readOk = aruco::DetectorParameters::readDetectorParameters(fs.root(), detectorParams); diff --git a/modules/aruco/samples/calibrate_camera_charuco.cpp b/modules/aruco/samples/calibrate_camera_charuco.cpp index 0026e6634a8..112f9059773 100644 --- a/modules/aruco/samples/calibrate_camera_charuco.cpp +++ b/modules/aruco/samples/calibrate_camera_charuco.cpp @@ -102,7 +102,7 @@ int main(int argc, char *argv[]) { if(parser.get("zt")) calibrationFlags |= CALIB_ZERO_TANGENT_DIST; if(parser.get("pc")) calibrationFlags |= CALIB_FIX_PRINCIPAL_POINT; - Ptr detectorParams; + Ptr detectorParams = aruco::DetectorParameters::create(); if(parser.has("dp")) { FileStorage fs(parser.get("dp"), FileStorage::READ); bool readOk = aruco::DetectorParameters::readDetectorParameters(fs.root(), detectorParams); diff --git a/modules/aruco/samples/detect_board.cpp b/modules/aruco/samples/detect_board.cpp index 357d5f799b1..0534a5e80ee 100644 --- a/modules/aruco/samples/detect_board.cpp +++ b/modules/aruco/samples/detect_board.cpp @@ -97,7 +97,7 @@ int main(int argc, char *argv[]) { } } - Ptr detectorParams; + Ptr detectorParams = aruco::DetectorParameters::create(); if(parser.has("dp")) { FileStorage fs(parser.get("dp"), FileStorage::READ); bool readOk = aruco::DetectorParameters::readDetectorParameters(fs.root(), detectorParams); @@ -199,7 +199,7 @@ int main(int argc, char *argv[]) { aruco::drawDetectedMarkers(imageCopy, rejected, noArray(), Scalar(100, 0, 255)); if(markersOfBoardDetected > 0) - aruco::drawAxis(imageCopy, camMatrix, distCoeffs, rvec, tvec, axisLength); + cv::drawFrameAxes(imageCopy, camMatrix, distCoeffs, rvec, tvec, axisLength); imshow("out", imageCopy); char key = (char)waitKey(waitTime); diff --git a/modules/aruco/samples/detect_board_charuco.cpp b/modules/aruco/samples/detect_board_charuco.cpp index b6064e72507..a6336377078 100644 --- a/modules/aruco/samples/detect_board_charuco.cpp +++ b/modules/aruco/samples/detect_board_charuco.cpp @@ -99,7 +99,7 @@ int main(int argc, char *argv[]) { } } - Ptr detectorParams; + Ptr detectorParams = aruco::DetectorParameters::create(); if(parser.has("dp")) { FileStorage fs(parser.get("dp"), FileStorage::READ); bool readOk = aruco::DetectorParameters::readDetectorParameters(fs.root(), detectorParams); @@ -213,7 +213,7 @@ int main(int argc, char *argv[]) { } if(validPose) - aruco::drawAxis(imageCopy, camMatrix, distCoeffs, rvec, tvec, axisLength); + cv::drawFrameAxes(imageCopy, camMatrix, distCoeffs, rvec, tvec, axisLength); imshow("out", imageCopy); char key = (char)waitKey(waitTime); diff --git a/modules/aruco/samples/detect_diamonds.cpp b/modules/aruco/samples/detect_diamonds.cpp index c6279a21428..bd8aa51d9eb 100644 --- a/modules/aruco/samples/detect_diamonds.cpp +++ b/modules/aruco/samples/detect_diamonds.cpp @@ -87,7 +87,7 @@ int main(int argc, char *argv[]) { bool autoScale = parser.has("as"); float autoScaleFactor = autoScale ? parser.get("as") : 1.f; - Ptr detectorParams; + Ptr detectorParams = aruco::DetectorParameters::create(); if(parser.has("dp")) { FileStorage fs(parser.get("dp"), FileStorage::READ); bool readOk = aruco::DetectorParameters::readDetectorParameters(fs.root(), detectorParams); @@ -219,8 +219,8 @@ int main(int argc, char *argv[]) { if(estimatePose) { for(unsigned int i = 0; i < diamondIds.size(); i++) - aruco::drawAxis(imageCopy, camMatrix, distCoeffs, rvecs[i], tvecs[i], - squareLength * 0.5f); + cv::drawFrameAxes(imageCopy, camMatrix, distCoeffs, rvecs[i], tvecs[i], + squareLength * 1.1f); } } diff --git a/modules/aruco/samples/detect_markers.cpp b/modules/aruco/samples/detect_markers.cpp index 30d6e55dd6e..5085a346ee5 100644 --- a/modules/aruco/samples/detect_markers.cpp +++ b/modules/aruco/samples/detect_markers.cpp @@ -80,7 +80,7 @@ int main(int argc, char *argv[]) { bool estimatePose = parser.has("c"); float markerLength = parser.get("l"); - Ptr detectorParams; + Ptr detectorParams = aruco::DetectorParameters::create(); if(parser.has("dp")) { FileStorage fs(parser.get("dp"), FileStorage::READ); bool readOk = aruco::DetectorParameters::readDetectorParameters(fs.root(), detectorParams); @@ -179,8 +179,7 @@ int main(int argc, char *argv[]) { if(estimatePose) { for(unsigned int i = 0; i < ids.size(); i++) - aruco::drawAxis(imageCopy, camMatrix, distCoeffs, rvecs[i], tvecs[i], - markerLength * 0.5f); + cv::drawFrameAxes(imageCopy, camMatrix, distCoeffs, rvecs[i], tvecs[i], markerLength * 1.5f, 2); } } diff --git a/modules/aruco/samples/tutorial_charuco_create_detect.cpp b/modules/aruco/samples/tutorial_charuco_create_detect.cpp index 33886e96728..89b538c8a01 100644 --- a/modules/aruco/samples/tutorial_charuco_create_detect.cpp +++ b/modules/aruco/samples/tutorial_charuco_create_detect.cpp @@ -72,7 +72,7 @@ static inline void detectCharucoBoardWithCalibrationPose() //! [pose] // if charuco pose is valid if (valid) - cv::aruco::drawAxis(imageCopy, cameraMatrix, distCoeffs, rvec, tvec, 0.1f); + cv::drawFrameAxes(imageCopy, cameraMatrix, distCoeffs, rvec, tvec, 0.1f); } } cv::imshow("out", imageCopy); diff --git a/modules/aruco/src/aruco.cpp b/modules/aruco/src/aruco.cpp index 5509c8aca54..9e61630fba8 100644 --- a/modules/aruco/src/aruco.cpp +++ b/modules/aruco/src/aruco.cpp @@ -295,11 +295,11 @@ static void _filterTooCloseCandidates(const vector< vector< Point2f > > &candida double minMarkerDistanceRate, bool detectInvertedMarker) { CV_Assert(minMarkerDistanceRate >= 0); - vector candGroup; candGroup.resize(candidatesIn.size(), -1); vector< vector > groupedCandidates; for(unsigned int i = 0; i < candidatesIn.size(); i++) { + bool isSingleContour = true; for(unsigned int j = i + 1; j < candidatesIn.size(); j++) { int minimumPerimeter = min((int)contoursIn[i].size(), (int)contoursIn[j].size() ); @@ -320,7 +320,7 @@ static void _filterTooCloseCandidates(const vector< vector< Point2f > > &candida // if mean square distance is too low, remove the smaller one of the two markers double minMarkerDistancePixels = double(minimumPerimeter) * minMarkerDistanceRate; if(distSq < minMarkerDistancePixels * minMarkerDistancePixels) { - + isSingleContour = false; // i and j are not related to a group if(candGroup[i]<0 && candGroup[j]<0){ // mark candidates with their corresponding group number @@ -351,6 +351,14 @@ static void _filterTooCloseCandidates(const vector< vector< Point2f > > &candida } } } + if (isSingleContour && candGroup[i] < 0) + { + candGroup[i] = (int)groupedCandidates.size(); + vector grouped; + grouped.push_back(i); + grouped.push_back(i); // step "save possible candidates" require minimum 2 elements + groupedCandidates.push_back(grouped); + } } // save possible candidates @@ -785,11 +793,11 @@ static void _getSingleMarkerObjectPoints(float markerLength, OutputArray _objPoi _objPoints.create(4, 1, CV_32FC3); Mat objPoints = _objPoints.getMat(); - // set coordinate system in the middle of the marker, with Z pointing out - objPoints.ptr< Vec3f >(0)[0] = Vec3f(-markerLength / 2.f, markerLength / 2.f, 0); - objPoints.ptr< Vec3f >(0)[1] = Vec3f(markerLength / 2.f, markerLength / 2.f, 0); - objPoints.ptr< Vec3f >(0)[2] = Vec3f(markerLength / 2.f, -markerLength / 2.f, 0); - objPoints.ptr< Vec3f >(0)[3] = Vec3f(-markerLength / 2.f, -markerLength / 2.f, 0); + // set coordinate system in the top-left corner of the marker, with Z pointing out + objPoints.ptr< Vec3f >(0)[0] = Vec3f(0.f, 0.f, 0); + objPoints.ptr< Vec3f >(0)[1] = Vec3f(markerLength, 0.f, 0); + objPoints.ptr< Vec3f >(0)[2] = Vec3f(markerLength, markerLength, 0); + objPoints.ptr< Vec3f >(0)[3] = Vec3f(0.f, markerLength, 0); } /** @@ -797,6 +805,7 @@ static void _getSingleMarkerObjectPoints(float markerLength, OutputArray _objPoi * @param nContours, contour-container */ static Point3f _interpolate2Dline(const std::vector& nContours){ + CV_Assert(nContours.size() >= 2); float minX, minY, maxX, maxY; minX = maxX = nContours[0].x; minY = maxY = nContours[0].y; @@ -847,21 +856,6 @@ static Point2f _getCrossPoint(Point3f nLine1, Point3f nLine2){ return Vec2f(A.solve(B).val); } -static void _distortPoints(vector& in, const Mat& camMatrix, const Mat& distCoeff) { - // trivial extrinsics - Matx31f Rvec(0,0,0); - Matx31f Tvec(0,0,0); - - // calculate 3d points and then reproject, so opencv makes the distortion internally - vector cornersPoints3d; - for (unsigned int i = 0; i < in.size(); i++){ - float x= (in[i].x - float(camMatrix.at(0, 2))) / float(camMatrix.at(0, 0)); - float y= (in[i].y - float(camMatrix.at(1, 2))) / float(camMatrix.at(1, 1)); - cornersPoints3d.push_back(Point3f(x,y,1)); - } - cv::projectPoints(cornersPoints3d, Rvec, Tvec, camMatrix, distCoeff, in); -} - /** * Refine Corners using the contour vector :: Called from function detectMarkers * @param nContours, contour-container @@ -869,13 +863,8 @@ static void _distortPoints(vector& in, const Mat& camMatrix, const * @param camMatrix, cameraMatrix input 3x3 floating-point camera matrix * @param distCoeff, distCoeffs vector of distortion coefficient */ -static void _refineCandidateLines(std::vector& nContours, std::vector& nCorners, const Mat& camMatrix, const Mat& distCoeff){ +static void _refineCandidateLines(std::vector& nContours, std::vector& nCorners){ vector contour2f(nContours.begin(), nContours.end()); - - if(!camMatrix.empty() && !distCoeff.empty()){ - undistortPoints(contour2f, contour2f, camMatrix, distCoeff); - } - /* 5 groups :: to group the edges * 4 - classified by its corner * extra group - (temporary) if contours do not begin with a corner @@ -893,10 +882,10 @@ static void _refineCandidateLines(std::vector& nContours, std::vector& nContours, std::vector &_dictionary, OutputArrayOfArrays _corners, OutputArray _ids, const Ptr &_params, - OutputArrayOfArrays _rejectedImgPoints, InputArrayOfArrays camMatrix, InputArrayOfArrays distCoeff) { + OutputArrayOfArrays _rejectedImgPoints) { CV_Assert(!_image.empty()); CV_Assert(_params->markerBorderBits > 0); @@ -1205,8 +1191,7 @@ void detectMarkers(InputArray _image, const Ptr &_dictionary, Output // do corner refinement using the contours for each detected markers parallel_for_(Range(0, _corners.cols()), [&](const Range& range) { for (int i = range.start; i < range.end; i++) { - _refineCandidateLines(contours[i], candidates[i], camMatrix.getMat(), - distCoeff.getMat()); + _refineCandidateLines(contours[i], candidates[i]); } }); @@ -1617,6 +1602,7 @@ Ptr Board::create(InputArrayOfArrays objPoints, const Ptr &di CV_Assert(objPoints.type() == CV_32FC3 || objPoints.type() == CV_32FC1); std::vector< std::vector< Point3f > > obj_points_vector; + Point3f rightBottomBorder = Point3f(0.f, 0.f, 0.f); for (unsigned int i = 0; i < objPoints.total(); i++) { std::vector corners; Mat corners_mat = objPoints.getMat(i); @@ -1626,7 +1612,11 @@ Ptr Board::create(InputArrayOfArrays objPoints, const Ptr &di CV_Assert(corners_mat.total() == 4); for (int j = 0; j < 4; j++) { - corners.push_back(corners_mat.at(j)); + const Point3f& corner = corners_mat.at(j); + corners.push_back(corner); + rightBottomBorder.x = std::max(rightBottomBorder.x, corner.x); + rightBottomBorder.y = std::max(rightBottomBorder.y, corner.y); + rightBottomBorder.z = std::max(rightBottomBorder.z, corner.z); } obj_points_vector.push_back(corners); } @@ -1635,6 +1625,7 @@ Ptr Board::create(InputArrayOfArrays objPoints, const Ptr &di ids.copyTo(res->ids); res->objPoints = obj_points_vector; res->dictionary = cv::makePtr(dictionary); + res->rightBottomBorder = rightBottomBorder; return res; } @@ -1670,20 +1661,19 @@ Ptr GridBoard::create(int markersX, int markersY, float markerLength, } // calculate Board objPoints - float maxY = (float)markersY * markerLength + (markersY - 1) * markerSeparation; for(int y = 0; y < markersY; y++) { for(int x = 0; x < markersX; x++) { - vector< Point3f > corners; - corners.resize(4); + vector corners(4); corners[0] = Point3f(x * (markerLength + markerSeparation), - maxY - y * (markerLength + markerSeparation), 0); + y * (markerLength + markerSeparation), 0); corners[1] = corners[0] + Point3f(markerLength, 0, 0); - corners[2] = corners[0] + Point3f(markerLength, -markerLength, 0); - corners[3] = corners[0] + Point3f(0, -markerLength, 0); + corners[2] = corners[0] + Point3f(markerLength, markerLength, 0); + corners[3] = corners[0] + Point3f(0, markerLength, 0); res->objPoints.push_back(corners); } } - + res->rightBottomBorder = Point3f(markersX * markerLength + markerSeparation * (markersX - 1), + markersY * markerLength + markerSeparation * (markersY - 1), 0.f); return res; } @@ -1735,15 +1725,6 @@ void drawDetectedMarkers(InputOutputArray _image, InputArrayOfArrays _corners, } - -/** - */ -void drawAxis(InputOutputArray _image, InputArray _cameraMatrix, InputArray _distCoeffs, InputArray _rvec, - InputArray _tvec, float length) -{ - drawFrameAxes(_image, _cameraMatrix, _distCoeffs, _rvec, _tvec, length, 3); -} - /** */ void drawMarker(const Ptr &dictionary, int id, int sidePixels, OutputArray _img, int borderBits) { @@ -1808,7 +1789,7 @@ void _drawPlanarBoardImpl(Board *_board, Size outSize, OutputArray _img, int mar // move top left to 0, 0 pf -= Point2f(minX, minY); pf.x = pf.x / sizeX * float(out.cols); - pf.y = (1.0f - pf.y / sizeY) * float(out.rows); + pf.y = pf.y / sizeY * float(out.rows); outCorners[j] = pf; } diff --git a/modules/aruco/src/charuco.cpp b/modules/aruco/src/charuco.cpp index 8397e916c48..b179bc63055 100644 --- a/modules/aruco/src/charuco.cpp +++ b/modules/aruco/src/charuco.cpp @@ -41,7 +41,6 @@ the use of this software, even if advised of the possibility of such damage. #include #include - namespace cv { namespace aruco { @@ -106,7 +105,7 @@ void CharucoBoard::draw(Size outSize, OutputArray _img, int marginSize, int bord double startX, startY; startX = squareSizePixels * double(x); - startY = double(chessboardZoneImg.rows) - squareSizePixels * double(y + 1); + startY = squareSizePixels * double(y); Mat squareZone = chessboardZoneImg.rowRange(int(startY), int(startY + squareSizePixels)) .colRange(int(startX), int(startX + squareSizePixels)); @@ -135,18 +134,17 @@ Ptr CharucoBoard::create(int squaresX, int squaresY, float squareL float diffSquareMarkerLength = (squareLength - markerLength) / 2; // calculate Board objPoints - for(int y = squaresY - 1; y >= 0; y--) { + for(int y = 0; y < squaresY; y++) { for(int x = 0; x < squaresX; x++) { if(y % 2 == x % 2) continue; // black corner, no marker here - vector< Point3f > corners; - corners.resize(4); + vector corners(4); corners[0] = Point3f(x * squareLength + diffSquareMarkerLength, - y * squareLength + diffSquareMarkerLength + markerLength, 0); + y * squareLength + diffSquareMarkerLength, 0); corners[1] = corners[0] + Point3f(markerLength, 0, 0); - corners[2] = corners[0] + Point3f(markerLength, -markerLength, 0); - corners[3] = corners[0] + Point3f(0, -markerLength, 0); + corners[2] = corners[0] + Point3f(markerLength, markerLength, 0); + corners[3] = corners[0] + Point3f(0, markerLength, 0); res->objPoints.push_back(corners); // first ids in dictionary int nextId = (int)res->ids.size(); @@ -164,7 +162,8 @@ Ptr CharucoBoard::create(int squaresX, int squaresY, float squareL res->chessboardCorners.push_back(corner); } } - + res->rightBottomBorder = Point3f(squaresX * squareLength, + squaresY * squareLength, 0.f); res->_getNearestMarkerCorners(); return res; @@ -701,15 +700,14 @@ double calibrateCameraCharuco(InputArrayOfArrays _charucoCorners, InputArrayOfAr void detectCharucoDiamond(InputArray _image, InputArrayOfArrays _markerCorners, InputArray _markerIds, float squareMarkerLengthRate, OutputArrayOfArrays _diamondCorners, OutputArray _diamondIds, - InputArray _cameraMatrix, InputArray _distCoeffs) { + InputArray _cameraMatrix, InputArray _distCoeffs, Ptr dictionary) { CV_Assert(_markerIds.total() > 0 && _markerIds.total() == _markerCorners.total()); const float minRepDistanceRate = 1.302455f; // create Charuco board layout for diamond (3x3 layout) - Ptr dict = getPredefinedDictionary(PREDEFINED_DICTIONARY_NAME(0)); - Ptr _charucoDiamondLayout = CharucoBoard::create(3, 3, squareMarkerLengthRate, 1., dict); + Ptr _charucoDiamondLayout = CharucoBoard::create(3, 3, squareMarkerLengthRate, 1., dictionary); vector< vector< Point2f > > diamondCorners; @@ -801,10 +799,10 @@ void detectCharucoDiamond(InputArray _image, InputArrayOfArrays _markerCorners, // reorder corners vector< Point2f > currentMarkerCornersReorder; currentMarkerCornersReorder.resize(4); - currentMarkerCornersReorder[0] = currentMarkerCorners[2]; - currentMarkerCornersReorder[1] = currentMarkerCorners[3]; - currentMarkerCornersReorder[2] = currentMarkerCorners[1]; - currentMarkerCornersReorder[3] = currentMarkerCorners[0]; + currentMarkerCornersReorder[0] = currentMarkerCorners[0]; + currentMarkerCornersReorder[1] = currentMarkerCorners[1]; + currentMarkerCornersReorder[2] = currentMarkerCorners[3]; + currentMarkerCornersReorder[3] = currentMarkerCorners[2]; diamondCorners.push_back(currentMarkerCornersReorder); diamondIds.push_back(markerId); diff --git a/modules/aruco/test/test_aruco_utils.hpp b/modules/aruco/test/test_aruco_utils.hpp new file mode 100644 index 00000000000..033d8f9bb4f --- /dev/null +++ b/modules/aruco/test/test_aruco_utils.hpp @@ -0,0 +1,127 @@ +// 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 "test_precomp.hpp" +namespace opencv_test { +namespace { + +static inline vector getAxis(InputArray _cameraMatrix, InputArray _distCoeffs, InputArray _rvec, + InputArray _tvec, float length, const float offset = 0.f) +{ + vector axis; + axis.push_back(Point3f(offset, offset, 0.f)); + axis.push_back(Point3f(length+offset, offset, 0.f)); + axis.push_back(Point3f(offset, length+offset, 0.f)); + axis.push_back(Point3f(offset, offset, length)); + vector axis_to_img; + projectPoints(axis, _rvec, _tvec, _cameraMatrix, _distCoeffs, axis_to_img); + return axis_to_img; +} + +static inline vector getMarkerById(int id, const vector >& corners, const vector& ids) +{ + for (size_t i = 0ull; i < ids.size(); i++) + if (ids[i] == id) + return corners[i]; + return vector(); +} + +static inline double deg2rad(double deg) { return deg * CV_PI / 180.; } + +/** + * @brief Get rvec and tvec from yaw, pitch and distance + */ +static inline void getSyntheticRT(double yaw, double pitch, double distance, Mat& rvec, Mat& tvec) { + rvec = Mat::zeros(3, 1, CV_64FC1); + tvec = Mat::zeros(3, 1, CV_64FC1); + + // rotate "scene" in pitch axis (x-axis) + Mat rotPitch(3, 1, CV_64FC1); + rotPitch.at(0) = -pitch; + rotPitch.at(1) = 0; + rotPitch.at(2) = 0; + + // rotate "scene" in yaw (y-axis) + Mat rotYaw(3, 1, CV_64FC1); + rotYaw.at(0) = 0; + rotYaw.at(1) = yaw; + rotYaw.at(2) = 0; + + // compose both rotations + composeRT(rotPitch, Mat(3, 1, CV_64FC1, Scalar::all(0)), rotYaw, + Mat(3, 1, CV_64FC1, Scalar::all(0)), rvec, tvec); + + // Tvec, just move in z (camera) direction the specific distance + tvec.at(0) = 0.; + tvec.at(1) = 0.; + tvec.at(2) = distance; +} + +/** + * @brief Project a synthetic marker + */ +static inline void projectMarker(Mat& img, Ptr board, int markerIndex, Mat cameraMatrix, Mat rvec, Mat tvec, + int markerBorder) { + // canonical image + Mat markerImg; + const int markerSizePixels = 100; + aruco::drawMarker(board->dictionary, board->ids[markerIndex], markerSizePixels, markerImg, markerBorder); + + // projected corners + Mat distCoeffs(5, 1, CV_64FC1, Scalar::all(0)); + vector< Point2f > corners; + + // get max coordinate of board + Point3f maxCoord = board->rightBottomBorder; + // copy objPoints + vector objPoints = board->objPoints[markerIndex]; + // move the marker to the origin + for (size_t i = 0; i < objPoints.size(); i++) + objPoints[i] -= (maxCoord / 2.f); + + projectPoints(objPoints, rvec, tvec, cameraMatrix, distCoeffs, corners); + + // get perspective transform + vector< Point2f > originalCorners; + originalCorners.push_back(Point2f(0, 0)); + originalCorners.push_back(Point2f((float)markerSizePixels, 0)); + originalCorners.push_back(Point2f((float)markerSizePixels, (float)markerSizePixels)); + originalCorners.push_back(Point2f(0, (float)markerSizePixels)); + Mat transformation = getPerspectiveTransform(originalCorners, corners); + + // apply transformation + Mat aux; + const char borderValue = 127; + warpPerspective(markerImg, aux, transformation, img.size(), INTER_NEAREST, BORDER_CONSTANT, + Scalar::all(borderValue)); + + // copy only not-border pixels + for (int y = 0; y < aux.rows; y++) { + for (int x = 0; x < aux.cols; x++) { + if (aux.at< unsigned char >(y, x) == borderValue) continue; + img.at< unsigned char >(y, x) = aux.at< unsigned char >(y, x); + } + } +} + + +/** + * @brief Get a synthetic image of GridBoard in perspective + */ +static inline Mat projectBoard(Ptr& board, Mat cameraMatrix, double yaw, double pitch, + double distance, Size imageSize, int markerBorder) { + + Mat rvec, tvec; + getSyntheticRT(yaw, pitch, distance, rvec, tvec); + + Mat img = Mat(imageSize, CV_8UC1, Scalar::all(255)); + for (unsigned int index = 0; index < board->ids.size(); index++) { + projectMarker(img, board.staticCast(), index, cameraMatrix, rvec, tvec, markerBorder); + } + + return img; +} + +} + +} diff --git a/modules/aruco/test/test_arucodetection.cpp b/modules/aruco/test/test_arucodetection.cpp index 9cc08bd011b..f6fad7f2864 100644 --- a/modules/aruco/test/test_arucodetection.cpp +++ b/modules/aruco/test/test_arucodetection.cpp @@ -655,4 +655,77 @@ TEST(CV_ArucoTutorial, can_find_gboriginal) } } +TEST(CV_ArucoDetectMarkers, regression_3192) +{ + Ptr dictionary = aruco::getPredefinedDictionary(aruco::DICT_4X4_50); + Ptr detectorParams = aruco::DetectorParameters::create(); + vector< int > markerIds; + vector > markerCorners; + string imgPath = cvtest::findDataFile("aruco/regression_3192.png"); + Mat image = imread(imgPath); + const size_t N = 2ull; + const int goldCorners[N][8] = { {345,120, 520,120, 520,295, 345,295}, {101,114, 270,112, 276,287, 101,287} }; + const int goldCornersIds[N] = { 6, 4 }; + map mapGoldCorners; + for (size_t i = 0; i < N; i++) + mapGoldCorners[goldCornersIds[i]] = goldCorners[i]; + + aruco::detectMarkers(image, dictionary, markerCorners, markerIds, detectorParams); + + ASSERT_EQ(N, markerIds.size()); + for (size_t i = 0; i < N; i++) + { + int arucoId = markerIds[i]; + ASSERT_EQ(4ull, markerCorners[i].size()); + ASSERT_TRUE(mapGoldCorners.find(arucoId) != mapGoldCorners.end()); + for (int j = 0; j < 4; j++) + { + EXPECT_NEAR(static_cast(mapGoldCorners[arucoId][j * 2]), markerCorners[i][j].x, 1.f); + EXPECT_NEAR(static_cast(mapGoldCorners[arucoId][j * 2 + 1]), markerCorners[i][j].y, 1.f); + } + } +} + +TEST(CV_ArucoDetectMarkers, regression_2492) +{ + Ptr dictionary = aruco::getPredefinedDictionary(aruco::DICT_5X5_50); + Ptr detectorParams = aruco::DetectorParameters::create(); + detectorParams->minMarkerDistanceRate = 0.026; + vector< int > markerIds; + vector > markerCorners; + string imgPath = cvtest::findDataFile("aruco/regression_2492.png"); + Mat image = imread(imgPath); + const size_t N = 8ull; + const int goldCorners[N][8] = { {179,139, 179,95, 223,95, 223,139}, {99,139, 99,95, 143,95, 143,139}, + {19,139, 19,95, 63,95, 63,139}, {256,140, 256,93, 303,93, 303,140}, + {256,62, 259,21, 300,23, 297,64}, {99,21, 143,17, 147,60, 103,64}, + {69,61, 28,61, 14,21, 58,17}, {174,62, 182,13, 230,19, 223,68} }; + const int goldCornersIds[N] = {13, 13, 13, 13, 1, 15, 14, 4}; + map > mapGoldCorners; + for (size_t i = 0; i < N; i++) + mapGoldCorners[goldCornersIds[i]].push_back(goldCorners[i]); + + aruco::detectMarkers(image, dictionary, markerCorners, markerIds, detectorParams); + + ASSERT_EQ(N, markerIds.size()); + for (size_t i = 0; i < N; i++) + { + int arucoId = markerIds[i]; + ASSERT_EQ(4ull, markerCorners[i].size()); + ASSERT_TRUE(mapGoldCorners.find(arucoId) != mapGoldCorners.end()); + float totalDist = 8.f; + for (size_t k = 0ull; k < mapGoldCorners[arucoId].size(); k++) + { + float dist = 0.f; + for (int j = 0; j < 4; j++) // total distance up to 4 points + { + dist += abs(mapGoldCorners[arucoId][k][j * 2] - markerCorners[i][j].x); + dist += abs(mapGoldCorners[arucoId][k][j * 2 + 1] - markerCorners[i][j].y); + } + totalDist = min(totalDist, dist); + } + EXPECT_LT(totalDist, 8.f); + } +} + }} // namespace diff --git a/modules/aruco/test/test_boarddetection.cpp b/modules/aruco/test/test_boarddetection.cpp index dc3a92ec4e4..ed4ee161719 100644 --- a/modules/aruco/test/test_boarddetection.cpp +++ b/modules/aruco/test/test_boarddetection.cpp @@ -38,119 +38,10 @@ the use of this software, even if advised of the possibility of such damage. #include "test_precomp.hpp" +#include "test_aruco_utils.hpp" namespace opencv_test { namespace { -static double deg2rad(double deg) { return deg * CV_PI / 180.; } - -/** - * @brief Get rvec and tvec from yaw, pitch and distance - */ -static void getSyntheticRT(double yaw, double pitch, double distance, Mat &rvec, Mat &tvec) { - - rvec = Mat(3, 1, CV_64FC1); - tvec = Mat(3, 1, CV_64FC1); - - // Rvec - // first put the Z axis aiming to -X (like the camera axis system) - Mat rotZ(3, 1, CV_64FC1); - rotZ.ptr< double >(0)[0] = 0; - rotZ.ptr< double >(0)[1] = 0; - rotZ.ptr< double >(0)[2] = -0.5 * CV_PI; - - Mat rotX(3, 1, CV_64FC1); - rotX.ptr< double >(0)[0] = 0.5 * CV_PI; - rotX.ptr< double >(0)[1] = 0; - rotX.ptr< double >(0)[2] = 0; - - Mat camRvec, camTvec; - composeRT(rotZ, Mat(3, 1, CV_64FC1, Scalar::all(0)), rotX, Mat(3, 1, CV_64FC1, Scalar::all(0)), - camRvec, camTvec); - - // now pitch and yaw angles - Mat rotPitch(3, 1, CV_64FC1); - rotPitch.ptr< double >(0)[0] = 0; - rotPitch.ptr< double >(0)[1] = pitch; - rotPitch.ptr< double >(0)[2] = 0; - - Mat rotYaw(3, 1, CV_64FC1); - rotYaw.ptr< double >(0)[0] = yaw; - rotYaw.ptr< double >(0)[1] = 0; - rotYaw.ptr< double >(0)[2] = 0; - - composeRT(rotPitch, Mat(3, 1, CV_64FC1, Scalar::all(0)), rotYaw, - Mat(3, 1, CV_64FC1, Scalar::all(0)), rvec, tvec); - - // compose both rotations - composeRT(camRvec, Mat(3, 1, CV_64FC1, Scalar::all(0)), rvec, - Mat(3, 1, CV_64FC1, Scalar::all(0)), rvec, tvec); - - // Tvec, just move in z (camera) direction the specific distance - tvec.ptr< double >(0)[0] = 0.; - tvec.ptr< double >(0)[1] = 0.; - tvec.ptr< double >(0)[2] = distance; -} - -/** - * @brief Project a synthetic marker - */ -static void projectMarker(Mat &img, Ptr &dictionary, int id, - vector< Point3f > markerObjPoints, Mat cameraMatrix, Mat rvec, Mat tvec, - int markerBorder) { - - - // canonical image - Mat markerImg; - const int markerSizePixels = 100; - aruco::drawMarker(dictionary, id, markerSizePixels, markerImg, markerBorder); - - // projected corners - Mat distCoeffs(5, 1, CV_64FC1, Scalar::all(0)); - vector< Point2f > corners; - projectPoints(markerObjPoints, rvec, tvec, cameraMatrix, distCoeffs, corners); - - // get perspective transform - vector< Point2f > originalCorners; - originalCorners.push_back(Point2f(0, 0)); - originalCorners.push_back(Point2f((float)markerSizePixels, 0)); - originalCorners.push_back(Point2f((float)markerSizePixels, (float)markerSizePixels)); - originalCorners.push_back(Point2f(0, (float)markerSizePixels)); - Mat transformation = getPerspectiveTransform(originalCorners, corners); - - // apply transformation - Mat aux; - const char borderValue = 127; - warpPerspective(markerImg, aux, transformation, img.size(), INTER_NEAREST, BORDER_CONSTANT, - Scalar::all(borderValue)); - - // copy only not-border pixels - for(int y = 0; y < aux.rows; y++) { - for(int x = 0; x < aux.cols; x++) { - if(aux.at< unsigned char >(y, x) == borderValue) continue; - img.at< unsigned char >(y, x) = aux.at< unsigned char >(y, x); - } - } -} - - -/** - * @brief Get a synthetic image of GridBoard in perspective - */ -static Mat projectBoard(Ptr &board, Mat cameraMatrix, double yaw, double pitch, - double distance, Size imageSize, int markerBorder) { - - Mat rvec, tvec; - getSyntheticRT(yaw, pitch, distance, rvec, tvec); - - Mat img = Mat(imageSize, CV_8UC1, Scalar::all(255)); - for(unsigned int m = 0; m < board->ids.size(); m++) { - projectMarker(img, board->dictionary, board->ids[m], board->objPoints[m], cameraMatrix, rvec, - tvec, markerBorder); - } - - return img; -} - enum class ArucoAlgParams { USE_DEFAULT = 0, @@ -170,6 +61,7 @@ class CV_ArucoBoardPose : public cvtest::BaseTest { params->useAruco3Detection = true; params->cornerRefinementMethod = aruco::CORNER_REFINE_SUBPIX; params->minSideLengthCanonicalImg = 16; + params->errorCorrectionRate = 0.8; } } @@ -180,7 +72,6 @@ class CV_ArucoBoardPose : public cvtest::BaseTest { void CV_ArucoBoardPose::run(int) { - int iter = 0; Mat cameraMatrix = Mat::eye(3, 3, CV_64FC1); Size imgSize(500, 500); @@ -193,35 +84,40 @@ void CV_ArucoBoardPose::run(int) { Mat distCoeffs(5, 1, CV_64FC1, Scalar::all(0)); // for different perspectives - for(double distance = 0.2; distance <= 0.4; distance += 0.2) { - for(int yaw = 0; yaw < 360; yaw += 100) { - for(int pitch = 30; pitch <= 90; pitch += 50) { + for(double distance = 0.2; distance <= 0.4; distance += 0.15) { + for(int yaw = -55; yaw <= 50; yaw += 25) { + for(int pitch = -55; pitch <= 50; pitch += 25) { for(unsigned int i = 0; i < gridboard->ids.size(); i++) gridboard->ids[i] = (iter + int(i)) % 250; int markerBorder = iter % 2 + 1; iter++; - // create synthetic image - Mat img = projectBoard(gridboard, cameraMatrix, deg2rad(pitch), deg2rad(yaw), distance, + Mat img = projectBoard(gridboard, cameraMatrix, deg2rad(yaw), deg2rad(pitch), distance, imgSize, markerBorder); - - vector< vector< Point2f > > corners; vector< int > ids; params->markerBorderBits = markerBorder; aruco::detectMarkers(img, dictionary, corners, ids, params); - if(ids.size() == 0) { - ts->printf(cvtest::TS::LOG, "Marker detection failed in Board test"); - ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH); - return; - } + ASSERT_EQ(ids.size(), gridboard->ids.size()); // estimate pose Mat rvec, tvec; aruco::estimatePoseBoard(corners, ids, board, cameraMatrix, distCoeffs, rvec, tvec); - // check result + // check axes + vector axes = getAxis(cameraMatrix, distCoeffs, rvec, tvec, gridboard->rightBottomBorder.x); + vector topLeft = getMarkerById(gridboard->ids[0], corners, ids); + ASSERT_NEAR(topLeft[0].x, axes[0].x, 2.f); + ASSERT_NEAR(topLeft[0].y, axes[0].y, 2.f); + vector topRight = getMarkerById(gridboard->ids[2], corners, ids); + ASSERT_NEAR(topRight[1].x, axes[1].x, 2.f); + ASSERT_NEAR(topRight[1].y, axes[1].y, 2.f); + vector bottomLeft = getMarkerById(gridboard->ids[6], corners, ids); + ASSERT_NEAR(bottomLeft[3].x, axes[2].x, 2.f); + ASSERT_NEAR(bottomLeft[3].y, axes[2].y, 2.f); + + // check estimate result for(unsigned int i = 0; i < ids.size(); i++) { int foundIdx = -1; for(unsigned int j = 0; j < gridboard->ids.size(); j++) { @@ -292,18 +188,16 @@ void CV_ArucoRefine::run(int) { // for different perspectives for(double distance = 0.2; distance <= 0.4; distance += 0.2) { - for(int yaw = 0; yaw < 360; yaw += 100) { - for(int pitch = 30; pitch <= 90; pitch += 50) { + for(int yaw = -60; yaw < 60; yaw += 30) { + for(int pitch = -60; pitch <= 60; pitch += 30) { for(unsigned int i = 0; i < gridboard->ids.size(); i++) gridboard->ids[i] = (iter + int(i)) % 250; int markerBorder = iter % 2 + 1; iter++; // create synthetic image - Mat img = projectBoard(gridboard, cameraMatrix, deg2rad(pitch), deg2rad(yaw), distance, + Mat img = projectBoard(gridboard, cameraMatrix, deg2rad(yaw), deg2rad(pitch), distance, imgSize, markerBorder); - - // detect markers vector< vector< Point2f > > corners, rejected; vector< int > ids; diff --git a/modules/aruco/test/test_charucodetection.cpp b/modules/aruco/test/test_charucodetection.cpp index c3a3c5cd094..9617b14872e 100644 --- a/modules/aruco/test/test_charucodetection.cpp +++ b/modules/aruco/test/test_charucodetection.cpp @@ -38,97 +38,10 @@ the use of this software, even if advised of the possibility of such damage. #include "test_precomp.hpp" +#include "test_aruco_utils.hpp" namespace opencv_test { namespace { -static double deg2rad(double deg) { return deg * CV_PI / 180.; } - -/** - * @brief Get rvec and tvec from yaw, pitch and distance - */ -static void getSyntheticRT(double yaw, double pitch, double distance, Mat &rvec, Mat &tvec) { - - rvec = Mat(3, 1, CV_64FC1); - tvec = Mat(3, 1, CV_64FC1); - - // Rvec - // first put the Z axis aiming to -X (like the camera axis system) - Mat rotZ(3, 1, CV_64FC1); - rotZ.ptr< double >(0)[0] = 0; - rotZ.ptr< double >(0)[1] = 0; - rotZ.ptr< double >(0)[2] = -0.5 * CV_PI; - - Mat rotX(3, 1, CV_64FC1); - rotX.ptr< double >(0)[0] = 0.5 * CV_PI; - rotX.ptr< double >(0)[1] = 0; - rotX.ptr< double >(0)[2] = 0; - - Mat camRvec, camTvec; - composeRT(rotZ, Mat(3, 1, CV_64FC1, Scalar::all(0)), rotX, Mat(3, 1, CV_64FC1, Scalar::all(0)), - camRvec, camTvec); - - // now pitch and yaw angles - Mat rotPitch(3, 1, CV_64FC1); - rotPitch.ptr< double >(0)[0] = 0; - rotPitch.ptr< double >(0)[1] = pitch; - rotPitch.ptr< double >(0)[2] = 0; - - Mat rotYaw(3, 1, CV_64FC1); - rotYaw.ptr< double >(0)[0] = yaw; - rotYaw.ptr< double >(0)[1] = 0; - rotYaw.ptr< double >(0)[2] = 0; - - composeRT(rotPitch, Mat(3, 1, CV_64FC1, Scalar::all(0)), rotYaw, - Mat(3, 1, CV_64FC1, Scalar::all(0)), rvec, tvec); - - // compose both rotations - composeRT(camRvec, Mat(3, 1, CV_64FC1, Scalar::all(0)), rvec, - Mat(3, 1, CV_64FC1, Scalar::all(0)), rvec, tvec); - - // Tvec, just move in z (camera) direction the specific distance - tvec.ptr< double >(0)[0] = 0.; - tvec.ptr< double >(0)[1] = 0.; - tvec.ptr< double >(0)[2] = distance; -} - -/** - * @brief Project a synthetic marker - */ -static void projectMarker(Mat &img, Ptr dictionary, int id, - vector< Point3f > markerObjPoints, Mat cameraMatrix, Mat rvec, Mat tvec, - int markerBorder) { - - - Mat markerImg; - const int markerSizePixels = 100; - aruco::drawMarker(dictionary, id, markerSizePixels, markerImg, markerBorder); - - Mat distCoeffs(5, 1, CV_64FC1, Scalar::all(0)); - vector< Point2f > corners; - projectPoints(markerObjPoints, rvec, tvec, cameraMatrix, distCoeffs, corners); - - vector< Point2f > originalCorners; - originalCorners.push_back(Point2f(0, 0)); - originalCorners.push_back(Point2f((float)markerSizePixels, 0)); - originalCorners.push_back(Point2f((float)markerSizePixels, (float)markerSizePixels)); - originalCorners.push_back(Point2f(0, (float)markerSizePixels)); - - Mat transformation = getPerspectiveTransform(originalCorners, corners); - - Mat aux; - const char borderValue = 127; - warpPerspective(markerImg, aux, transformation, img.size(), INTER_NEAREST, BORDER_CONSTANT, - Scalar::all(borderValue)); - - // copy only not-border pixels - for(int y = 0; y < aux.rows; y++) { - for(int x = 0; x < aux.cols; x++) { - if(aux.at< unsigned char >(y, x) == borderValue) continue; - img.at< unsigned char >(y, x) = aux.at< unsigned char >(y, x); - } - } -} - /** * @brief Get a synthetic image of Chessboard in perspective */ @@ -145,7 +58,7 @@ static Mat projectChessboard(int squaresX, int squaresY, float squareSize, Size float startX = float(x) * squareSize; vector< Point3f > squareCorners; - squareCorners.push_back(Point3f(startX, startY, 0)); + squareCorners.push_back(Point3f(startX, startY, 0) - Point3f(squaresX*squareSize/2.f, squaresY*squareSize/2.f, 0.f)); squareCorners.push_back(squareCorners[0] + Point3f(squareSize, 0, 0)); squareCorners.push_back(squareCorners[0] + Point3f(squareSize, squareSize, 0)); squareCorners.push_back(squareCorners[0] + Point3f(0, squareSize, 0)); @@ -180,8 +93,8 @@ static Mat projectCharucoBoard(Ptr &board, Mat cameraMatrix // project markers Mat img = Mat(imageSize, CV_8UC1, Scalar::all(255)); - for(unsigned int m = 0; m < board->ids.size(); m++) { - projectMarker(img, board->dictionary, board->ids[m], board->objPoints[m], cameraMatrix, rvec, + for(unsigned int indexMarker = 0; indexMarker < board->ids.size(); indexMarker++) { + projectMarker(img, board.staticCast(), indexMarker, cameraMatrix, rvec, tvec, markerBorder); } @@ -222,7 +135,7 @@ void CV_CharucoDetection::run(int) { Ptr dictionary = aruco::getPredefinedDictionary(aruco::DICT_6X6_250); Ptr board = aruco::CharucoBoard::create(4, 4, 0.03f, 0.015f, dictionary); - cameraMatrix.at< double >(0, 0) = cameraMatrix.at< double >(1, 1) = 650; + cameraMatrix.at< double >(0, 0) = cameraMatrix.at< double >(1, 1) = 600; cameraMatrix.at< double >(0, 2) = imgSize.width / 2; cameraMatrix.at< double >(1, 2) = imgSize.height / 2; @@ -230,15 +143,15 @@ void CV_CharucoDetection::run(int) { // for different perspectives for(double distance = 0.2; distance <= 0.4; distance += 0.2) { - for(int yaw = 0; yaw < 360; yaw += 100) { - for(int pitch = 30; pitch <= 90; pitch += 50) { + for(int yaw = -55; yaw <= 50; yaw += 25) { + for(int pitch = -55; pitch <= 50; pitch += 25) { int markerBorder = iter % 2 + 1; iter++; // create synthetic image Mat rvec, tvec; - Mat img = projectCharucoBoard(board, cameraMatrix, deg2rad(pitch), deg2rad(yaw), + Mat img = projectCharucoBoard(board, cameraMatrix, deg2rad(yaw), deg2rad(pitch), distance, imgSize, markerBorder, rvec, tvec); // detect markers @@ -269,7 +182,13 @@ void CV_CharucoDetection::run(int) { // check results vector< Point2f > projectedCharucoCorners; - projectPoints(board->chessboardCorners, rvec, tvec, cameraMatrix, distCoeffs, + + // copy chessboardCorners + vector copyChessboardCorners = board->chessboardCorners; + // move copyChessboardCorners points + for (size_t i = 0; i < copyChessboardCorners.size(); i++) + copyChessboardCorners[i] -= board->rightBottomBorder / 2.f; + projectPoints(copyChessboardCorners, rvec, tvec, cameraMatrix, distCoeffs, projectedCharucoCorners); for(unsigned int i = 0; i < charucoIds.size(); i++) { @@ -327,16 +246,16 @@ void CV_CharucoPoseEstimation::run(int) { Mat distCoeffs(5, 1, CV_64FC1, Scalar::all(0)); // for different perspectives - for(double distance = 0.2; distance <= 0.4; distance += 0.2) { - for(int yaw = 0; yaw < 360; yaw += 100) { - for(int pitch = 30; pitch <= 90; pitch += 50) { + for(double distance = 0.2; distance <= 0.3; distance += 0.1) { + for(int yaw = -55; yaw <= 50; yaw += 25) { + for(int pitch = -55; pitch <= 50; pitch += 25) { int markerBorder = iter % 2 + 1; iter++; // get synthetic image Mat rvec, tvec; - Mat img = projectCharucoBoard(board, cameraMatrix, deg2rad(pitch), deg2rad(yaw), + Mat img = projectCharucoBoard(board, cameraMatrix, deg2rad(yaw), deg2rad(pitch), distance, imgSize, markerBorder, rvec, tvec); // detect markers @@ -347,11 +266,7 @@ void CV_CharucoPoseEstimation::run(int) { params->markerBorderBits = markerBorder; aruco::detectMarkers(img, dictionary, corners, ids, params); - if(ids.size() == 0) { - ts->printf(cvtest::TS::LOG, "Marker detection failed"); - ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH); - return; - } + ASSERT_EQ(ids.size(), board->ids.size()); // interpolate charuco corners vector< Point2f > charucoCorners; @@ -372,8 +287,19 @@ void CV_CharucoPoseEstimation::run(int) { distCoeffs, rvec, tvec); - // check result + // check axes + const float offset = (board->getSquareLength() - board->getMarkerLength()) / 2.f; + vector axes = getAxis(cameraMatrix, distCoeffs, rvec, tvec, board->getSquareLength(), offset); + vector topLeft = getMarkerById(board->ids[0], corners, ids); + ASSERT_NEAR(topLeft[0].x, axes[1].x, 3.f); + ASSERT_NEAR(topLeft[0].y, axes[1].y, 3.f); + vector bottomLeft = getMarkerById(board->ids[2], corners, ids); + ASSERT_NEAR(bottomLeft[0].x, axes[2].x, 3.f); + ASSERT_NEAR(bottomLeft[0].y, axes[2].y, 3.f); + + // check estimate result vector< Point2f > projectedCharucoCorners; + projectPoints(board->chessboardCorners, rvec, tvec, cameraMatrix, distCoeffs, projectedCharucoCorners); @@ -434,9 +360,9 @@ void CV_CharucoDiamondDetection::run(int) { Mat distCoeffs(5, 1, CV_64FC1, Scalar::all(0)); // for different perspectives - for(double distance = 0.3; distance <= 0.3; distance += 0.2) { - for(int yaw = 0; yaw < 360; yaw += 100) { - for(int pitch = 30; pitch <= 90; pitch += 30) { + for(double distance = 0.2; distance <= 0.3; distance += 0.1) { + for(int yaw = -50; yaw <= 50; yaw += 25) { + for(int pitch = -50; pitch <= 50; pitch += 25) { int markerBorder = iter % 2 + 1; for(int i = 0; i < 4; i++) @@ -445,7 +371,7 @@ void CV_CharucoDiamondDetection::run(int) { // get synthetic image Mat rvec, tvec; - Mat img = projectCharucoBoard(board, cameraMatrix, deg2rad(pitch), deg2rad(yaw), + Mat img = projectCharucoBoard(board, cameraMatrix, deg2rad(yaw), deg2rad(pitch), distance, imgSize, markerBorder, rvec, tvec); // detect markers @@ -485,14 +411,21 @@ void CV_CharucoDiamondDetection::run(int) { vector< Point2f > projectedDiamondCorners; - projectPoints(board->chessboardCorners, rvec, tvec, cameraMatrix, distCoeffs, + + // copy chessboardCorners + vector copyChessboardCorners = board->chessboardCorners; + // move copyChessboardCorners points + for (size_t i = 0; i < copyChessboardCorners.size(); i++) + copyChessboardCorners[i] -= board->rightBottomBorder / 2.f; + + projectPoints(copyChessboardCorners, rvec, tvec, cameraMatrix, distCoeffs, projectedDiamondCorners); vector< Point2f > projectedDiamondCornersReorder(4); - projectedDiamondCornersReorder[0] = projectedDiamondCorners[2]; - projectedDiamondCornersReorder[1] = projectedDiamondCorners[3]; - projectedDiamondCornersReorder[2] = projectedDiamondCorners[1]; - projectedDiamondCornersReorder[3] = projectedDiamondCorners[0]; + projectedDiamondCornersReorder[0] = projectedDiamondCorners[0]; + projectedDiamondCornersReorder[1] = projectedDiamondCorners[1]; + projectedDiamondCornersReorder[2] = projectedDiamondCorners[3]; + projectedDiamondCornersReorder[3] = projectedDiamondCorners[2]; for(unsigned int i = 0; i < 4; i++) { @@ -514,10 +447,10 @@ void CV_CharucoDiamondDetection::run(int) { // check result vector< Point2f > projectedDiamondCornersPose; vector< Vec3f > diamondObjPoints(4); - diamondObjPoints[0] = Vec3f(-squareLength / 2.f, squareLength / 2.f, 0); - diamondObjPoints[1] = Vec3f(squareLength / 2.f, squareLength / 2.f, 0); - diamondObjPoints[2] = Vec3f(squareLength / 2.f, -squareLength / 2.f, 0); - diamondObjPoints[3] = Vec3f(-squareLength / 2.f, -squareLength / 2.f, 0); + diamondObjPoints[0] = Vec3f(0.f, 0.f, 0); + diamondObjPoints[1] = Vec3f(squareLength, 0.f, 0); + diamondObjPoints[2] = Vec3f(squareLength, squareLength, 0); + diamondObjPoints[3] = Vec3f(0.f, squareLength, 0); projectPoints(diamondObjPoints, estimatedRvec[0], estimatedTvec[0], cameraMatrix, distCoeffs, projectedDiamondCornersPose); @@ -681,24 +614,20 @@ TEST(Charuco, testCharucoCornersCollinear_false) TEST(Charuco, testBoardSubpixelCoords) { cv::Size res{500, 500}; - cv::Mat K = (cv::Mat_(3,3) << - 0.5*res.width, 0, 0.5*res.width, - 0, 0.5*res.height, 0.5*res.height, - 0, 0, 1); // load board image with corners at round values cv::String testImagePath = cvtest::TS::ptr()->get_data_path() + "aruco/" + "trivial_board_detection.png"; Mat img = imread(testImagePath); - cv::Mat expected_corners = (cv::Mat_(9,2) << - 200, 300, - 250, 300, - 300, 300, + cv::Mat expected_corners = (cv::Mat_(9, 2) << + 200, 200, + 250, 200, + 300, 200, 200, 250, 250, 250, 300, 250, - 200, 200, - 250, 200, - 300, 200 + 200, 300, + 250, 300, + 300, 300 ); cv::Mat gray; @@ -713,24 +642,16 @@ TEST(Charuco, testBoardSubpixelCoords) std::vector ids; std::vector> corners, rejected; - cv::aruco::detectMarkers(gray, dict, corners, ids, params, rejected, K); + cv::aruco::detectMarkers(gray, dict, corners, ids, params, rejected); ASSERT_EQ(ids.size(), size_t(8)); cv::Mat c_ids, c_corners; - cv::aruco::interpolateCornersCharuco(corners, ids, gray, board, c_corners, c_ids, K); - cv::Mat corners_reshaped = c_corners.reshape(1); - - ASSERT_EQ(c_corners.rows, expected_corners.rows); - EXPECT_NEAR(0, cvtest::norm(expected_corners, c_corners.reshape(1), NORM_INF), 1e-3); - - c_ids = cv::Mat(); - c_corners = cv::Mat(); cv::aruco::interpolateCornersCharuco(corners, ids, gray, board, c_corners, c_ids); - corners_reshaped = c_corners.reshape(1); + cv::Mat corners_reshaped = c_corners.reshape(1); ASSERT_EQ(c_corners.rows, expected_corners.rows); - EXPECT_NEAR(0, cvtest::norm(expected_corners, c_corners.reshape(1), NORM_INF), 1e-3); + EXPECT_NEAR(0, cvtest::norm(expected_corners, c_corners.reshape(1), NORM_INF), 4e-2); } TEST(CV_ArucoTutorial, can_find_choriginal) @@ -868,7 +789,8 @@ TEST(Charuco, issue_14014) ASSERT_EQ(corners.size(), 19ull); EXPECT_EQ(Size(4, 1), corners[0].size()); // check dimension of detected corners - ASSERT_EQ(rejectedPoints.size(), 21ull); + size_t numRejPoints = rejectedPoints.size(); + ASSERT_EQ(rejectedPoints.size(), 26ull); // optional check to track regressions EXPECT_EQ(Size(4, 1), rejectedPoints[0].size()); // check dimension of detected corners aruco::refineDetectedMarkers(img, board, corners, ids, rejectedPoints); @@ -876,7 +798,7 @@ TEST(Charuco, issue_14014) ASSERT_EQ(corners.size(), 20ull); EXPECT_EQ(Size(4, 1), corners[0].size()); // check dimension of rejected corners after successfully refine - ASSERT_EQ(rejectedPoints.size(), 20ull); + ASSERT_EQ(rejectedPoints.size() + 1, numRejPoints); EXPECT_EQ(Size(4, 1), rejectedPoints[0].size()); // check dimension of rejected corners after successfully refine } diff --git a/modules/aruco/tutorials/aruco_board_detection/aruco_board_detection.markdown b/modules/aruco/tutorials/aruco_board_detection/aruco_board_detection.markdown index af3a0173a16..95d3a3dc3a4 100644 --- a/modules/aruco/tutorials/aruco_board_detection/aruco_board_detection.markdown +++ b/modules/aruco/tutorials/aruco_board_detection/aruco_board_detection.markdown @@ -83,13 +83,14 @@ The parameters of estimatePoseBoard are: markers provided in ```markerCorners``` and ```markerIds``` should be used, since only the markers whose ids are listed in the ```Board::ids``` structure are considered. -The ```drawAxis()``` function can be used to check the obtained pose. For instance: +The ```drawFrameAxes()``` function can be used to check the obtained pose. For instance: -![Board with axis](images/gbmarkersaxis.png) +![Board with axis](images/gbmarkersaxis.jpg) And this is another example with the board partially occluded: ![Board with occlusions](images/gbocclusion.png) +@note The center and direction of the axes has been changed As it can be observed, although some markers have not been detected, the Board pose can still be estimated from the rest of markers. @@ -111,7 +112,7 @@ with all the markers in the same plane and in a grid layout, as in the following Concretely, the coordinate system in a Grid Board is positioned in the board plane, centered in the bottom left corner of the board and with the Z pointing out, like in the following image (X:red, Y:green, Z:blue): -![Board with axis](images/gbaxis.png) +![Board with axis](images/gbaxis.jpg) A ```GridBoard``` object can be defined using the following parameters: @@ -197,7 +198,7 @@ Finally, a full example of board detection: // if at least one board marker detected if(valid > 0) - cv::aruco::drawAxis(imageCopy, cameraMatrix, distCoeffs, rvec, tvec, 0.1); + cv::drawFrameAxes(imageCopy, cameraMatrix, distCoeffs, rvec, tvec, 0.1); } cv::imshow("out", imageCopy); diff --git a/modules/aruco/tutorials/aruco_board_detection/images/gbaxis.jpg b/modules/aruco/tutorials/aruco_board_detection/images/gbaxis.jpg new file mode 100644 index 00000000000..1aa40faf2c5 Binary files /dev/null and b/modules/aruco/tutorials/aruco_board_detection/images/gbaxis.jpg differ diff --git a/modules/aruco/tutorials/aruco_board_detection/images/gbaxis.png b/modules/aruco/tutorials/aruco_board_detection/images/gbaxis.png deleted file mode 100644 index 1e4e69fc9b9..00000000000 Binary files a/modules/aruco/tutorials/aruco_board_detection/images/gbaxis.png and /dev/null differ diff --git a/modules/aruco/tutorials/aruco_board_detection/images/gbmarkersaxis.jpg b/modules/aruco/tutorials/aruco_board_detection/images/gbmarkersaxis.jpg new file mode 100644 index 00000000000..a94536d3876 Binary files /dev/null and b/modules/aruco/tutorials/aruco_board_detection/images/gbmarkersaxis.jpg differ diff --git a/modules/aruco/tutorials/aruco_board_detection/images/gbmarkersaxis.png b/modules/aruco/tutorials/aruco_board_detection/images/gbmarkersaxis.png deleted file mode 100644 index 50593ba820e..00000000000 Binary files a/modules/aruco/tutorials/aruco_board_detection/images/gbmarkersaxis.png and /dev/null differ diff --git a/modules/aruco/tutorials/aruco_board_detection/images/singlemarkersaxes.jpg b/modules/aruco/tutorials/aruco_board_detection/images/singlemarkersaxes.jpg new file mode 100644 index 00000000000..c7a1db44d20 Binary files /dev/null and b/modules/aruco/tutorials/aruco_board_detection/images/singlemarkersaxes.jpg differ diff --git a/modules/aruco/tutorials/aruco_detection/aruco_detection.markdown b/modules/aruco/tutorials/aruco_detection/aruco_detection.markdown index be9bb690f51..ee0518b0c53 100644 --- a/modules/aruco/tutorials/aruco_detection/aruco_detection.markdown +++ b/modules/aruco/tutorials/aruco_detection/aruco_detection.markdown @@ -299,7 +299,7 @@ inputImage.copyTo(outputImage); for (int i = 0; i < rvecs.size(); ++i) { auto rvec = rvecs[i]; auto tvec = tvecs[i]; - cv::aruco::drawAxis(outputImage, cameraMatrix, distCoeffs, rvec, tvec, 0.1); + cv::drawFrameAxes(outputImage, cameraMatrix, distCoeffs, rvec, tvec, 0.1); } @endcode @@ -337,7 +337,7 @@ while (inputVideo.grab()) { cv::aruco::estimatePoseSingleMarkers(corners, 0.05, cameraMatrix, distCoeffs, rvecs, tvecs); // draw axis for each marker for(int i=0; i